打印

[小应用]GD32F103+MPU9150 四轴飞行器第一步:9DOF姿态融合

[复制链接]
楼主: zkcaptain
手机看帖
扫描二维码
随时随地手机跟帖
341
基于加速度计、陀螺仪和磁力计的姿态融合,能够实时更新四元数,并由四元数解算出姿态角。

使用特权

评论回复
342
angerbird| | 2015-3-22 12:25 | 只看该作者
传感器MPU9150(INVENSENSE公司的,单芯片内集成了加速度计、陀螺仪和磁力计,并且内置DMP用于姿态融合,不过只融合了加速度计和陀螺仪,具有自校准功能。

使用特权

评论回复
343
angerbird| | 2015-3-22 12:27 | 只看该作者
轴向的定义跟初始化四元数和最后结算的欧拉角有关,跟IMUUpdate四元数更新算法无关,换句话说,不管你的轴向如何定义,IMUUpdate随便用,但是初始化四元数的公式和最后结算欧拉角的公式要做适当的改变。

使用特权

评论回复
344
angerbird| | 2015-3-22 12:31 | 只看该作者
常见的轴向定义是绕x轴旋转是Roll,绕y轴旋转是Pitch,绕z轴旋转是Yaw。

使用特权

评论回复
345
zkcaptain|  楼主 | 2015-3-23 16:53 | 只看该作者
whoyzf 发表于 2015-3-20 23:20
这初始化的公式,我觉得用方向余弦矩阵来推导,更容易理解一点。求初始的俯仰角和翻滚角时,用n系(地球坐 ...

对于黑色字体部分的讨论,我认为只要自己理解起来方便,三个初始化角计算正确,哪种方式都可以,不影响结果。

对于红色字体部分的讨论,初始化时加速度计和磁力计不用归一化也是可以的,只要单位保持一致即可,我代码里是将加速度计的单位定为g,磁力计直接使用的就是原始数据的单位,即便是经过归一化后,在求反正切反正弦时,归一化的值也会被分子分母约分掉的。
归一化的目的:一个是方便在重复运算时提高运算效率;另一个是在写代码时方便同一量纲,不用再反复考虑单位。初始化时不用考虑这两点。当然,做了归一化也可以。

使用特权

评论回复
346
whoyzf| | 2015-3-29 01:59 | 只看该作者
本帖最后由 whoyzf 于 2015-3-29 17:02 编辑

楼主,为啥用你的程序,磁力计输出都是正值啊?


你的程序中貌似没有区分磁力计输出的正负值,手册上是正负值是按如下区分的。

不知道是不是我哪里理解错了。希望楼主解答下,多谢。

使用特权

评论回复
347
zkcaptain|  楼主 | 2015-3-30 09:42 | 只看该作者
whoyzf 发表于 2015-3-29 01:59
楼主,为啥用你的程序,磁力计输出都是正值啊?

你看到的mag[0]、mag[1]、mag[2]应该是补码的形式,一般传感器的原始数据都是以补码的形式出来的,具体的正负需要自己转换。

使用特权

评论回复
348
whoyzf| | 2015-3-30 16:00 | 只看该作者
zkcaptain 发表于 2015-3-30 09:42
你看到的mag[0]、mag[1]、mag[2]应该是补码的形式,一般传感器的原始数据都是以补码的形式出来的,具体的 ...

那你的代码呢?我也是用MPU9150啊。刚才换了个芯片,读出的值没有变化,不知道什么原因。参考的是你的代码 。

使用特权

评论回复
349
whoyzf| | 2015-3-30 16:27 | 只看该作者
zkcaptain 发表于 2015-3-30 09:42
你看到的mag[0]、mag[1]、mag[2]应该是补码的形式,一般传感器的原始数据都是以补码的形式出来的,具体的 ...

你的代码中貌似没有区分这个开关的符号啊。

使用特权

评论回复
350
zkcaptain|  楼主 | 2015-3-30 16:38 | 只看该作者
whoyzf 发表于 2015-3-30 16:27
你的代码中貌似没有区分这个开关的符号啊。

你再琢磨一下程序吧,我测过我的程序,是有正负的

你也可以把你的代码和磁力计输出截个图上来看看

使用特权

评论回复
351
whoyzf| | 2015-3-30 17:18 | 只看该作者
zkcaptain 发表于 2015-3-30 16:38
你再琢磨一下程序吧,我测过我的程序,是有正负的

你也可以把你的代码和磁力计输出截个图上来看看 ...

我看的是你的代码。刚才重新焊了个芯片,却发现,磁力计的值不会变了。。。。。好纠结。。。

使用特权

评论回复
352
zkcaptain|  楼主 | 2015-3-30 17:29 | 只看该作者
whoyzf 发表于 2015-3-30 17:18
我看的是你的代码。刚才重新焊了个芯片,却发现,磁力计的值不会变了。。。。。好纠结。。。 ...

是会有这种情况出现,重焊即可。

使用特权

评论回复
353
whoyzf| | 2015-3-30 17:32 | 只看该作者
zkcaptain 发表于 2015-3-30 17:29
是会有这种情况出现,重焊即可。

我刚刚是重焊了下一个新的芯片,结果数值不会变,就第一个数值会变下。那是因为没焊好吗

使用特权

评论回复
354
whoyzf| | 2015-3-30 18:37 | 只看该作者
本帖最后由 whoyzf 于 2015-3-30 18:57 编辑
zkcaptain 发表于 2015-3-30 17:29
是会有这种情况出现,重焊即可。

你的芯片多少钱买的啊?
我的芯片28元买的,貌似坏的。
数据是这样的,在变动

使用特权

评论回复
355
zkcaptain|  楼主 | 2015-3-31 09:02 | 只看该作者
whoyzf 发表于 2015-3-30 18:37
你的芯片多少钱买的啊?
我的芯片28元买的,貌似坏的。
数据是这样的,在变动

我当时是80元/片买的,不过价格没有参考意义,刚开始我也是怎么调程序都不行,后来重新焊了一下,发现原来是焊接问题,这个东西感觉不是很好焊,也不能说就是芯片坏了。

最好能把代码的图上来看看,数据图看不出来具体原因,如果你能确定不是代码问题,就从焊接上找原因吧,软件方面,目前遇到唯一问题就是要在数据ready后再读取才行,不然数据会出错或者保持不变,也就是说读取磁力计速率不能太快,你看下这个内置磁力计的output rate是多少

使用特权

评论回复
356
whoyzf| | 2015-3-31 10:49 | 只看该作者
zkcaptain 发表于 2015-3-31 09:02
我当时是80元/片买的,不过价格没有参考意义,刚开始我也是怎么调程序都不行,后来重新焊了一下,发现原 ...

嗯。我买个了模块,先看看,程序就是移植你的,几乎没改。

使用特权

评论回复
357
whoyzf| | 2015-4-1 14:48 | 只看该作者
zkcaptain 发表于 2015-3-31 09:02
我当时是80元/片买的,不过价格没有参考意义,刚开始我也是怎么调程序都不行,后来重新焊了一下,发现原 ...

mxMAX:678.000000;mxMIN:428.000000; myMAX:354.000000;myMIN:83.000000; mzMAX:132.000000;mzMIN:-130.000000;

我发现可能是我的板子上有干扰,芯片拆下来焊在模块上有正负值。但是把模块放在板子上就和这个值差不多。根据上面可以看出每个轴的变化范围大致相同的。250左右。应该可以认为是正常的吧?

使用特权

评论回复
358
whoyzf| | 2015-4-1 15:21 | 只看该作者
本帖最后由 whoyzf 于 2015-4-1 15:31 编辑

你的代码中貌似没有对磁力计的输出值进行补码。为了验证正确性
我改了你的代码,让其二进制输出。
signed short int mag[3];
  unsigned char tmp[7], data_write[1];
  
  tmp[6]=0x00;
  data_write[0]=0x01;
  i2cread(Compass_Addr, Compass_ST1, 1, tmp+6);
  if(tmp[6] == 1)
  {
    i2cread(Compass_Addr, Compass_HXL, 6, tmp);
        mag[0] = (((signed short int)tmp[1]) << 8) | tmp[0];
    mag[1] = (((signed short int)tmp[3]) << 8) | tmp[2];
    mag[2] = (((signed short int)tmp[5]) << 8) | tmp[4];

//          mag[0] = ((long)mag[0] * mag_sens_adj_val[0]) >> 8;  //ÁéÃô¶Èµ÷Õû
//    mag[1] = ((long)mag[1] * mag_sens_adj_val[1]) >> 8;
//    mag[2] = ((long)mag[2] * mag_sens_adj_val[2]) >> 8;

          init_mx =(float)mag[1];                //ת»»×ø±êÖá                                
    init_my =(float)mag[0];
    init_mz =(float)-mag[2];
        i2cwrite(Compass_Addr, Compass_CNTL, 1, data_write);         //¿ªÆôcompass£ºsingle measurement mode
  }
printf("my:%d(0x%02x%02x) mx:%d(0x%02x%02x) mz:%d(0x%02x%02x) \r\n",mag[0],tmp[1],tmp[0],mag[1],tmp[3],tmp[2],mag[2],tmp[5],tmp[4])        ;
结果发现
my:-495(0xfe11) mx:225(0x00e1) mz:-727(0xfd29)
my:-501(0xfe0b) mx:219(0x00db) mz:-729(0xfd27)
my:-491(0xfe15) mx:221(0x00dd) mz:-729(0xfd27)
my:-496(0xfe10) mx:216(0x00d8) mz:-726(0xfd2a)
my:-495(0xfe11) mx:225(0x00e1) mz:-729(0xfd27)
my:-487(0xfe19) mx:223(0x00df) mz:-727(0xfd29)
my:-496(0xfe10) mx:218(0x00da) mz:-728(0xfd28)
my:-492(0xfe14) mx:220(0x00dc) mz:-726(0xfd2a)
还真的符合,不知道具体原理。可以在计算机中的存储就是以那个补码形式存在吧

使用特权

评论回复
359
whoyzf| | 2015-4-1 19:17 | 只看该作者
楼主你好。我做的姿态解算,不知道怎么老是变化慢。比如动下飞机,要过一会才缓慢地变化到相应的位置。不知道这是为什么?

使用特权

评论回复
360
whoyzf| | 2015-4-4 11:06 | 只看该作者
zkcaptain 发表于 2015-3-23 16:53
对于黑色字体部分的讨论,我认为只要自己理解起来方便,三个初始化角计算正确,哪种方式都可以,不影响结 ...

init_Pitch=  asin(init_ay);              //init_Pitch = asin(ay / 1);      
那这个式子可能是有问题的。因为当地的重力加速度不一定为1吧。如果用
init_Pitch=  asin(init_ay/(init_ax*init_ax+init_ay*init_ay+init_az*init_az));  会更准确点吧?

使用特权

评论回复
发新帖 我要提问
您需要登录后才可以回帖 登录 | 注册

本版积分规则