原码放送,MPU6050的DMP用STM32成功的实现,内有移植问题说明
本帖最后由 _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的人应该很清楚怎么处理,你把读出来的数据除一下就行了。
还有一点是别忘记了IIC的初始化。。。。。。。。。。。这个错我犯了。。。。。。。。。。。。。。。
请问有没有接中断,我刚下了烧了一下发现PE口的没有数据,修改成PB口的可以,然后出来的是
我自己的dmp的代码是遇到
本帖最后由 _spetrel 于 2013-6-14 11:25 编辑
windless 发表于 2013-6-14 11:18 static/image/common/back.gif
请问有没有接中断,我刚下了烧了一下发现PE口的没有数据,修改成PB口的可以,然后出来的是
我自己的dmp的代 ...
你的问题我碰到过,我当时发现的问题是因为IIC总线没初始化,后来初始化后就解决了。我没有接中断。 我朋友中午用他的板子测试一下,下午看看,不过还是要谢谢你。我也准备测试完了发代码,可是最后一步dmp_read_fifo有问题,所以你看上面我的那个出来的数据是有问题的。 顶起,记下。 感谢无私的楼主 好人一生平安{:victory:} 支持原创。好人一生平安
好东西,谢谢分享! 支持楼主! 收藏了!!! 顶,楼主好人。 记号,支持楼主,刚买模块还没到 大好人楼主!!! 顶楼主!! 此贴怎能不顶 楼主牛{:victory:} 谢谢分享。。 楼主大牛啊啊 int类型的函数,返回0成功,小于0失败
bool类型的函数,返回1成功,0失败
STM32是32位的机器,肯定是用int类型比较合理
如果觉得有歧义,那说明只是8位单片机写多了,
特意改成return TRUE,感觉是画蛇添足了,言多勿怪。 lz,为什么iic 我初始化了之后,串口也配置好了,但是输出的全是?号,就是一次上电只输出一个?,不知道有么有人遇到跟我一样的问题。 感谢分享,对于lz的无私我只能先顶一个,技术问题看了源码再说 太好啦%%%%%*** aki_studio 发表于 2013-6-15 06:31 static/image/common/back.gif
int类型的函数,返回0成功,小于0失败
bool类型的函数,返回1成功,0失败
STM32是32位的机器,肯定是用int类 ...
没有觉得怪啊,因为IIC的底层驱动与DMP里面返回值类型刚好相反,而且代码里面涉及到许多的用返回值做判断的代码,直接改肯定不方便啊,这里是写了个中间的函数避免大量的处理。还有从占内存的角度来看,似乎用char要好些吧,char也能表示正负啊 windless 发表于 2013-6-15 09:39 static/image/common/back.gif
lz,为什么iic 我初始化了之后,串口也配置好了,但是输出的全是?号,就是一次上电只输出一个?,不知道有么 ...
你这个是不是串口有问题啊,你可以注释掉代码只单独的输出串口信息啊。 为什么我的f4只要循环有延时程序,输出就是0int main(void)
{
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
//if (SysTick_Config(SystemCoreClock / 21000000))
//{
// /* Capture error */
// while (1);
// }
//InitMPU6050();
USART2_Config();
USART3_Config();
TIM_Config();
TIM1_INT();
EXTILine_Config();
EXTI_GenerateSWInterrupt(EXTI_Line0);
EXTI_GenerateSWInterrupt(EXTI_Line1);
GPIO_ResetBits(GPIOD, GPIO_Pin_1);
GPIO_ResetBits(GPIOD, GPIO_Pin_2);
GPIO_SetBits(GPIOE, GPIO_Pin_12);
GPIO_SetBits(GPIOE, GPIO_Pin_10);
i2cInit();
result = mpu_init();
//mpu_init();
//mpu_set_sensor
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_configure_fifo
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
//mpu_set_sample_rate
mpu_set_sample_rate(DEFAULT_MPU_HZ);
//dmp_load_motion_driver_firmvare
dmp_load_motion_driver_firmware();
//dmp_set_orientation
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
//dmp_enable_feature
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
DMP_FEATURE_GYRO_CAL);
//dmp_set_fifo_rate
dmp_set_fifo_rate(DEFAULT_MPU_HZ);
run_self_test();
mpu_set_dmp_state(1);
while(1)
{
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
if (sensors & INV_WXYZ_QUAT )
{
q0 = quat / q30;
q1 = quat / q30;
q2 = quat / q30;
q3 = quat / q30;
Pitch= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
}
printf("%f\n",Pitch);
printf("%f\n",Roll);
printf("%f\n\n\n",Yaw);
//send_packet(PACKET_TYPE_QUAT, quat);
} 是传感器休眠了吗 本帖最后由 sirbai 于 2013-6-17 13:15 编辑
算出的三个数是三个轴与重力夹角吗 好帖mark hah最近正在研究中 先顶一个 楼主,你解析出来的数据飘不飘。比如我平着放置,第一个数据为-6,然后动一下之后再放平,数据就飘了,恢复到-6是个渐进的变化,不知道楼主数据如何? 主哇,你这个是6轴的的试试外接传感器9轴的??
好贴,不错先赞一个,夜晚细细看! 本帖最后由 _spetrel 于 2013-6-18 20:57 编辑
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验,在没有数据的时候,能用Slider控件来调试看看立方体的姿态变化 。
数据的通信格式为:"S" +正负号+ pitch +正负号+ roll + 正负号+yaw + 'E',
pitch,roll,yaw为unint16_t 型的数据,先发高位,后发低位,正负号用0表示正号,用其它的表示负号,正负号用uint8_t来表示。
之前发的贴子发现这个DMP很不稳定,初始化要复位好几次,把I2C的延时加长一点也不行,不知道是什么原因。还有就是这个DMP采样输出的数据其实是很不连续的,看图
,觉得做个插值会不会好点。还有我发现程序跑的时间长了,Yaw角会一直的增大,这个问题不知道各位测试的时候碰到没。
http://v.youku.com/v_show/id_XNTcyNDU1ODcy.html
这个是调试的时候的视频,看起来还可以。
准备放弃DMP方案,还是去研究算法算了,采样率只能到200HZ啊。。。。。 _spetrel 发表于 2013-6-18 20:54 static/image/common/back.gif
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验 ...
没这么慢吧??? _spetrel 发表于 2013-6-18 20:54 static/image/common/back.gif
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验 ...
楼主滤波算法研究好了别忘了分享下啊 本帖最后由 stm8s 于 2013-6-19 21:28 编辑
支持原创。好人一生平安
楼主滤波算法研究好了别忘了分享下啊 +1 sirbai 发表于 2013-6-16 23:34 static/image/common/back.gif
算出的三个数是三个轴与重力夹角吗
同问。。。 顶起!!感谢分享。 感谢分享~ 马上动身
既然可以通过直接读FIFO得到四元数,为什么不能像读取普通的寄存器一样来操作?还要做那么复杂的驱动程序?看的头晕啊…… if (tmp & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo();
return -2;
}
我在这里老出错,后来发现不能单步运行,联续就没问题了 找了好久,这是个好东西,谢过了 感谢分享,好东西,正要用 留个记号 楼主,程序在运行到run_self_test();时,打印显示“bias has not been modified ”,请问这是自检部分吗?这一步没过的话,会不会有影响? 感谢分享~ ~~~ 好东西,先留一个,说不定什么时候会用到哇。 lancefang 发表于 2013-7-12 14:05 static/image/common/back.gif
楼主,程序在运行到run_self_test();时,打印显示“bias has not been modified ”,请问这是自检部分吗? ...
同问,同求解答,目前出来的4个数都是0 lancefang 发表于 2013-7-12 14:05 static/image/common/back.gif
楼主,程序在运行到run_self_test();时,打印显示“bias has not been modified ”,请问这是自检部分吗? ...
搞定了,这个不用管它 先标记一下,好帖子啊 lancefang 发表于 2013-7-12 14:05 static/image/common/back.gif
楼主,程序在运行到run_self_test();时,打印显示“bias has not been modified ”,请问这是自检部分吗? ...
楼主的也是这样,我之前也是这样,回头仔细看楼主的截图就清楚了。 R2D1 发表于 2013-7-17 11:13 static/image/common/back.gif
楼主的也是这样,我之前也是这样,回头仔细看楼主的截图就清楚了。
你搞到哪一步了?上浆了没? 楼主程序,调出的数据。感谢楼主 lancefang 发表于 2013-7-17 12:25 static/image/common/back.gif
你搞到哪一步了?上浆了没?
进度比较慢,机架还没买。 楼主,我的yaw数据总有-8°左右的漂移,啥原因呢 谢谢分享,学习了{:smile:} 好帖,顶。 buck 发表于 2013-7-19 11:48 static/image/common/back.gif
偏航是正常的啊,没参考系修正,一般用地磁作航向修正
好的,谢了哈,昨天调试,放着跑了一个多小时吧,发现偏航角从0增加到-9°,然后就从-9°缓慢变化到+18°左右,看样子不上磁力计这玩意肯定是用不了啊 _spetrel 发表于 2013-6-18 20:54 static/image/common/back.gif
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验 ...
你好,你这个上位机的源码方便发一份吗? 200HZ,如果数据准确的话也够用了,就怕拿到的数据还需要处理,精度估计就不行了。 _spetrel 发表于 2013-6-18 20:54 static/image/common/back.gif
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验 ...
您好,你的这个上位机的源代码可以发给我一份吗?如果方便的话请发到邮箱1139301489@qq.com,在下不胜感激! 从DMP里面出来的数据有几种
1.姿态四元数,二选一:DMP_FEATURE_LP_QUAT(只用陀螺仪计算得到)/DMP_FEATURE_6X_LP_QUAT(加速度和陀螺仪融合)
2.加速度数据,DMP_FEATURE_SEND_RAW_ACCEL
3.陀螺仪数据.二选一: DMP_FEATURE_SEND_RAW_GYRO(原始数据) / DMP_FEATURE_SEND_CAL_GYRO(自动校准后数据)
3.时间戳
加入自动校准功能(DMP_FEATURE_GYRO_CAL)后,姿态可以做到非常稳定,自动校准功能在静止8秒后自动进行.
果断顶起,学习了! int mpu_read_fifo_stream(unsigned short length, unsigned char *data,
unsigned char *more)
{
unsigned char tmp;
unsigned short fifo_count;
if (!st.chip_cfg.dmp_on)
return -1;
if (!st.chip_cfg.sensors)
return -1;
if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))
return -1;
fifo_count = (tmp << 8) | tmp;
if (fifo_count < length) {
more = 0;
return -1;
}
if (fifo_count > (st.hw->max_fifo >> 1)) {
/* FIFO is 50% full, better check overflow bit. */
if (i2c_read(st.hw->addr, st.reg->int_status, 1, tmp))
return -1;
if (tmp & BIT_FIFO_OVERFLOW) {
mpu_reset_fifo();
return -1;
}
}
if (i2c_read(st.hw->addr, st.reg->fifo_r_w, length, data))
return -1;
more = fifo_count / length - 1;
return 0;
}
程序一直在红字那段错误退出,找很久都不知道哪里出错,看I2C逻辑应该没问题,读写都很顺利。LZ有没有遇过这种情况。 将读FIFO周期缩短了就没有溢出错误这问题了。但是输出结果的欧拉角乱跳,毫无规律。都不知道啥问题。我用的是STM8的,应该不关事吧 收下了,谢谢lz LZ 大无私……………………………… lgnativs 发表于 2013-7-23 14:32 static/image/common/back.gif
从DMP里面出来的数据有几种
1.姿态四元数,二选一:DMP_FEATURE_LP_QUAT(只用陀螺仪计算得到)/DMP_FEATURE_6X ...
自动校准是只校准陀螺仪,那磁力计上电的时候也应该校准吧? 楼主,我用9150的片子,程序还有要改动的地方吗?我在结构体里加上了磁力计的几行程序,偏航还是有点漂,新手求指教啊 学习ING,,,,期待俺的四轴早日起飞 MPU_ID1:68
mPU_ID2:68
rate_div:100
bias has not been modified ......
mpu initialization complete......
mpu_set_sensor complete ......
mpu_configure_fifo complete ......
mpu_set_sample_rate error ......
dmp_load_motion_driver_firmware come across error ......
dmp_set_orientation complete ......
dmp_enable_feature complete ......
dmp_set_fifo_rate complete ......
bias has not been modified ......
mpu_set_dmp_state complete ......
为什么我MPU
mpu_set_sample_rate error ......
dmp_load_motion_driver_firmware come across error ......
老是失败的? MPU的SlaveAddress不是0xD0吗? 为什么库函数里定义是SlaveAddress是 0x68 , i2cread(0x68, 0x75, 1, &read_out) 读回来的是0x68?
write_in = 100;
i2cwrite(0x68,0x19, 1, &write_in);
i2cread(0x68, 0x19, 1, &read_out);
printf("rate_div:%d\n",read_out);
读回来是rate_div:100 , 为什么还是显示mpu_set_sample_rate error ......
dmp_load_motion_driver_firmware come across error ......这句是哪里出错了? 好人啊!!! mark......
顶一个...{:lol:} 最近都在看,mark~~ 悔恨当初没有仔细看楼主的帖子,基本上说到得问题我都遇上了。。。 楼主好人,顶起这个真心给力 源码的话,官方是公开了的。
关于其固件的算法,不知道楼主怎么理解。
V5.1用户手册说的太模糊。 楼主 您好 我最近也在玩 STM32MPU6050 ,下载了您的代码 但总是有问题 !想让您给我指点指点我的qq:1149840258 求助 本帖最后由 NJ8888 于 2013-8-22 21:25 编辑
一行行拷贝,用新塘400KHz的硬件IIC搞了下,大约快3000行 一直关注数据融合 我用新塘123芯片移植了下,IIC是硬件的,400kHz,输出通过公式换成欧拉角传PC _spetrel 发表于 2013-6-18 20:54 static/image/common/back.gif
attach://120109.rar这个是我用MFC+openGL写的一个上位机,只能显示姿态,波特率9600,8个数据位,无校验 ...
这个是什么软件,挺好的 好东西,谢谢分享! Pitch不旋转时,ROLL和YAW数据正常,但是只要Pitch旋转一定角度后,ROLL和YAW数据就不正常了;
平放时数据(Pitch = -0.027330Roll = 0.818385Yaw = -0.941145)
往ROLL旋转一定角度后,(Pitch = 0.751704Roll = -42.787109Yaw = -0.686597)
保持ROLL不变,此时只往Pitch方向旋转(Pitch = -35.260311Roll = -42.027378Yaw = 19.004519) 串口出来上是:ID read as 0 indicates device is either incompatible or an MPU3050.
一直是这个问题啊,怎么办啊啊 终于找到源代码了,好好研究一下。{:victory:} PITCH转动到90度后,ROLL和YAW乱跳,这个是万向锁。 DMP最大的问题貌似就是速度了。这个在飞机上会出大问题的。 请问楼主
#define delay_ms delay_ms
#define get_ms get_ms
这里的 get_ms是实现什么功能啊? 不用处理没关系吗? 用什么东西别写的啊?
{:biggrin:} 默默七 发表于 2013-9-1 11:27 static/image/common/back.gif
DMP最大的问题貌似就是速度了。这个在飞机上会出大问题的。
你的意思是不用DMP?不过我看过有人四轴上用dmp的呀,速度很成问题吗 l楼主,我不知道你用什么环境编译的,我用Source in sight看你的程序,怎么都找不到twi_write(a, b, d, c);twi_read(a, b, d, c);MPL_LOGI这几个函数和变量的定义在哪,楼主是不是调用了什么库函数? ivanl 发表于 2013-8-1 15:37 static/image/common/back.gif
楼主,我用9150的片子,程序还有要改动的地方吗?我在结构体里加上了磁力计的几行程序,偏航还是有点漂,新 ...
请问这位哥们是用的STM32+MPU9150吗?9150用的是数字的还是模拟的?我们被限定用瑞萨编MPU9150,各种尴尬,串口刚刚写好 LZ好人,不过自己拿来有点看不懂,有没详细的解释 楼主,我移植了你的程序,但是最后得到的Pitch,Roll,Yaw值都飘的很大,你的飘吗 windless 发表于 2013-6-18 17:53 static/image/common/back.gif
楼主,你解析出来的数据飘不飘。比如我平着放置,第一个数据为-6,然后动一下之后再放平,数据就飘了,恢复 ...
我解析出来的Pitch,Roll,Yaw都飘,放着不动,飘得很厉害,不知道哪里搞错了 楼主 请问若要用到MPU6050里面的DMP 是不是还需要 把HMC5883和MPU6050连接起来啊,不然罗盘数据他内部的DMP怎么读取到呢?
求解。。。。 不错 记下啦 谢谢楼主分享。 NJ8888 发表于 2013-8-22 21:23 static/image/common/back.gif
一行行拷贝,用新塘400KHz的硬件IIC搞了下,大约快3000行
哪款芯片,ARM核的吗?