【分享】大神教你在STM32上实现MPU6050的DMP

之前移植匿名四轴的代码，但是发现求出的Pitch与Roll一直不断的增加，静止放着也不行。

后来偶然看到利用DMP读取MPU6050的中的四元素，就打算移植过来。花了几天总算成功，现在把移植过程分享给大家，另附带源码

DMP Test.rar (1.99 MB, 下载次数: 4310 )

注：I2C我用的是匿名四轴的。但是要注意，官方的那个编程方式很像Linux，调用一个函数，成功的时候返回的是0，异常时返回的是－1，－2等。这个地方需要注意一下。

/////////////////////////////////////////////////////////////////////////////////
int8_t i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
        if(i2cWriteBuffer(addr,reg,len,data))
        {
                return TRUE;//0
        }
        else
        {
                return FALSE;//-1
        }
        //return FALSE;
}
int8_t i2cread(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
        if(i2cRead(addr,reg,len,buf))
        {
                return TRUE;//0
        }
        else
        {
                return FALSE;//-1
        }
        //return FALSE;
}
//////////////////////////////////////////////////////////////////////////////////

const struct hw_s hw = {
    .addr           = 0x68,
    .max_fifo       = 1024,
    .num_reg        = 118,
    .temp_sens      = 340,
    .temp_offset    = -521,
    .bank_size      = 256
#if defined AK89xx_SECONDARY
    ,.compass_fsr    = AK89xx_FSR
#endif
};
上面这块是GNU C 的初始化， 在MDK里面是不能用的，要改成下面的
const struct hw_s hw={
  0x68,         //addr
  1024,         //max_fifo
  118,         //num_reg
  340,         //temp_sens
  -521,         //temp_offset
  256         //bank_size
};

对了陀螺仪的状态初始化
static struct gyro_state_s st={
  ®,
  &hw,
  {0},
  &test
};
这里的一块初始化放到mpu_init函数里面去了，这里可以不管，所以初始化为0

struct gyro_reg_s {
    unsigned char who_am_i;
    unsigned char rate_div;
    unsigned char lpf;
    unsigned char prod_id;
    unsigned char user_ctrl;
    unsigned char fifo_en;
    unsigned char gyro_cfg;
    unsigned char accel_cfg;

    //unsigned char accel_cfg2;

    //unsigned char lp_accel_odr;

    unsigned char motion_thr;
    unsigned char motion_dur;
    unsigned char fifo_count_h;
    unsigned char fifo_r_w;
    unsigned char raw_gyro;
    unsigned char raw_accel;
    unsigned char temp;
    unsigned char int_enable;
    unsigned char dmp_int_status;
    unsigned char int_status;

    //unsigned char accel_intel;
......
}这个结构体里面的那三个必须注释掉，不然你直接把标准GNU C的初始化改成标准C的，那么数据会出现偏移的，上面的三个在库里面没有初始化，你对着源码看看就知道。

#define i2c_write   i2cwrite
#define i2c_read    i2cread
#define delay_ms    delay_ms
#define get_ms      get_ms
这个是接口，其实上面的英文说得很清楚，之前我还很纠结这个get_ms到底怎么写，后来发现没用，可以对着源来430的代码看，这个get_ms就用了一次。

读出来的四元数数据是Q30的，就是放大2的30次方，做过定点数FFT的人应该很清楚怎么处理，你把读出来的数据除一下就行了。

还有一点是别忘记了IIC的初始化。。。。。。。。。。。这个错我犯了。。。。。。。。。。。。。。。

本帖最后由 _spetrel 于 2013-6-14 10:23 编辑

之前用匿名四轴的代码称植过来发现求出的Pitch与Roll一直不断的增加，不知道什么原因，静止放着也不行。

看了不少四元数的资料，能看懂，但是结合数据融合又糊了。加上卡尔漫数据也就那样，可能是我处理得有问题。偶然翻帖看到了DMP，于是打算移植过来看看。

花了两三的时间总算成功了。现在把一些移植过程中的问题给大家分享一下，顺带的附上源码。

I2C我用的是匿名四轴的，能用。但是要注意，官方的那个编程方式很像Linux，应该说用的是GNU C。在Linux下，调用一个函数，成功的时候返回的是0，异常时返回的是－1，－2等。但是一般人写的驱动成功都用1表示，失败用0表示。所以这点要注意一下。

/////////////////////////////////////////////////////////////////////////////////

int8_t i2cwrite(uint8_t addr, uint8_t reg,uint8_t len, uint8_t * data)

{

       if(i2cWriteBuffer(addr,reg,len,data))

       {

                return TRUE;//0

       }

       else

       {

                return FALSE;//-1

       }

       //return FALSE;

}

int8_t i2cread(uint8_t addr, uint8_t reg,uint8_t len, uint8_t *buf)

{

       if(i2cRead(addr,reg,len,buf))

       {

                return TRUE;//0

       }

       else

       {

                return FALSE;//-1

       }

       //return FALSE;

}

//////////////////////////////////////////////////////////////////////////////////

const struct hw_s hw = {

   .addr           = 0x68,

   .max_fifo       = 1024,

   .num_reg        = 118,

   .temp_sens      = 340,

   .temp_offset    = -521,

   .bank_size      = 256

#if defined AK89xx_SECONDARY

   ,.compass_fsr    = AK89xx_FSR

#endif

};

上面这块是GNU C 的初始化，在MDK里面是不能用的，要改成下面的

const struct hw_s hw={

0x68,         //addr

1024,         //max_fifo

118,         //num_reg

340,         //temp_sens

-521,         //temp_offset

256         //bank_size

};

对了陀螺仪的状态初始化

static struct gyro_state_s st={

®,

&hw,

{0},

&test

};

这里的一块初始化放到mpu_init函数里面去了，这里可以不管，所以初始化为0

struct gyro_reg_s {

   unsigned char who_am_i;

   unsigned char rate_div;

   unsigned char lpf;

   unsigned char prod_id;

   unsigned char user_ctrl;

   unsigned char fifo_en;

   unsigned char gyro_cfg;

   unsigned char accel_cfg;

   //unsigned char accel_cfg2;

   //unsigned char lp_accel_odr;

   unsigned char motion_thr;

   unsigned char motion_dur;

   unsigned char fifo_count_h;

   unsigned char fifo_r_w;

   unsigned char raw_gyro;

   unsigned char raw_accel;

   unsigned char temp;

   unsigned char int_enable;

   unsigned char dmp_int_status;

   unsigned char int_status;

   //unsigned char accel_intel;

......

}这个结构体里面的那三个必须注释掉，不然你直接把标准GNU C的初始化改成标准C的，那么数据会出现偏移的，上面的三个在库里面没有初始化，你对着源码看看就知道。

#define i2c_write   i2cwrite

#define i2c_read    i2cread

#define delay_ms    delay_ms

#define get_ms      get_ms

这个是接口，其实上面的英文说得很清楚，之前我还很纠结这个get_ms到底怎么写，后来发现没用，可以对着源来430的代码看，这个get_ms就用了一次。

读出来的四元数数据是Q30的，就是放大2的30次方，做过定点数FFT的人应该很清楚怎么处理，你把读出来的数据除一下就行了。