哈,写个mpu6050驱动经验

中山野鬼 发布于 2015/11/12 00:57
阅读 1K+
收藏 1

要玩加速度了,结果发现,加速度测量一坨shit。自检纠偏后,在z轴上,重力只有0.83个G,相反,如果将传感器转90度,让重力到y轴或x轴上,都是能呈现1个G。于是,上了invensense的官网,发现有6.12的版本。和我手上12年的版本是同源的。

对比一下,不知道手上的代码是给路人甲改过了,还是原先就有问题,在自校验的函数中确实存在差异。依葫芦画瓢的将最新版本的代码和我自己重写的驱动合并(为了降低代码量,我压缩了很多算法逻辑,所以无法直接使用)。

可以说,6.12版本确实不错。z轴已经基本上到1G了。而且纠正了原先我发现的两个错之一。


上面的图,左边是6.12版本的,右边是网络路人甲的12年版本。

,另一个错,在不同编译器下,会有不同的错误情况,貌似还没纠正。具体位置我忘了,因为我现在的代码如上说的,已经重写,并不能和6.12版本或12年的版本进行直接行对比,但记得是针对角速度z轴的,去除该bug对ypr中航向有精度提高。

有了上面的新版本的自校验,对加速度的获取数值,可以得到如下结果:

左边是y轴,右边是z轴,这里y 轴的精度在千分之4左右(x轴和y轴差不多),z轴的精度在千分之13左右。简单说,如果是2G档,落在z轴,总存在200多的误差(16位精度)。

其自校验的大体原理就是,静止下读一批数据上来,然后做平均,将这个平均值,在z轴上减去1G(减去重力加速度),得到的就是3个轴的DC量,将芯片中针对accel 的offset的寄存器中的值读取出来,减去这个DC量,再存回去。而offset里的值会对采样到加速度数值进行修正操作。

回到误差上来,看似千分之几的误差不大,但作为 dc 常量就算相当的大了。这里百分比是针对1G的,如果我的物理课不是语文老师客串的,0.1g的加速度,近似为1米/秒^2。如果加速度是0.1米/秒^2,那么就是上述的百分之一。 0.01G的dc误差,100秒,就可以让静止模块变成有了1G的加速度,想有跳楼的感觉?不要多,100秒就够。。。

这里就要说说,mpu6050鬼扯的地方。在自校验的时候,dmp没有开(如果我没记错)。而在实际采样是,dmp开的,读取fifo中的加速度数据,即便已经做了补偿,但仍然存在dc误差。不得已,重新写了自校验的算法。大体如下:

1、对offset 寄存器清0,等于第一次读取上来的是没有纠正的数据,求平均放入offset中,这处于dmp没有打开下。

2、dmp正常工作时,连续读取一定数据,求平均,和offset中的数据进行合并。

3、重复第二步的工作。就是再迭代一次。

三次的不同结果如下,只做第一步,如下,浮点数是当前获取值与1G的比例,冒号后面的整数是读取的值,三组分别为x,y,z轴:

简直惨不忍睹,误差特别是z轴到了0.08,这里没给出连续64个采样的求平均,z轴平均值大约在160上下。做了第二步,此时又补偿了一些值,则有如下数据:

上述执行过程,实际是又补偿了连续64个采样的平均值,按照道理应该能就完工了,但实际上z轴还是存在大约-15的dc系数,这就是我说mpu6050鬼扯的地方。

如果上述三步都做,则有如下结果:

还是有噪声,不过dc分量基本趋于1,0,-1这几个值。这样进行积分求速度时,累计误差就很小了。

不得不吐槽的是,如果前面版本,12年版的是路人甲的(由xx网站上开源下载的),确实代码改动过(或许没改,本身自校验的逻辑就有问题),导致z轴重力加速度不是1,而是0.83也就算了。这拿的官方最新的驱动,实在想不通,为何不做迭代校验,把自家器件的性能发挥出来。哈,或许是为了留一手?留个毛线啊! 当然也可能是我对这个片子驱动设置本身有问题,导致不得不使用这种迭代方法。反正12年版本,未重写时,这问题就已经存在。

================

补充追加一个在运行中的自纠正算法。如果有需要的朋友,可以借鉴一下。大体思想如下:

由于在运动或者温度变化后,加速度传感器会有积累出来的DC常量,这种常量对积分形成速度是灾难。因此我们需要通过一个检测方式,启动校验补偿。这里就给出一个假设,假设设备本身非静止下,各角度的加速度的差值会大于一定值,

因此,我们检测两次相邻采样的差值,如果大于限定值,则不做求dc的累计操作,此时假设在运动状态,并没有静止,如果小于限定值,则进行累计。累计到64次后,进行平均。

对3个轴平均后的dc常量的绝对值做累加,如果大于2则,则进行自校验补偿。

下面给的三个图,第一个是用手摇啊摇,(顺带喷一句,不想误差出来,很容易出来,想误差出来,摇半天才出来),z轴存在了均值为4的误差(64采样求平均,去除重力),此时发生一次自动校验。第一列是x轴的加速度数据,1个单位约是0.000488个G 。到100个以上基本上是被摇动中。第二列是x轴采样数据做平均(窗口宽度是64),随后4列分别是y轴和z轴的。你可以看出,静止后,z轴的dc到了-4 。

于是就有了第二副图,忽略前面3行,是姿态角度。

此处,x,y,z轴分别补偿 -1,1,-4,进行纠偏,经过200ms左右,采样数据dc恢复到了0附近。


对应源码如下,供参考:


static void MPU6050_recv_FIFO(uint8_t *data,uint8_t length){
    uint8_t *pbuf = fifo_buf[fifo_No];
    int16_t *paccel = (int16_t*)(pbuf + 16);
    uint8_t  i;
    uint8_t _t;
    int16_t tmp;
    buf_cpy(pbuf,data,length);
    pbuf += 16;
    
    for (i = 0 ; i < 6; i+=2){
        _t = pbuf[0];
        pbuf[0] = pbuf[1];
        pbuf[1] = _t;
        pbuf+=2;
    }
#ifdef USED_AUTOCHECK_ACCEL
    
    uint8_t zflag;
    if (test_bias > TEST_COUNT){
        goto _MPU6050_recv_FIFO_clean;
    }
    
    for (i = 0 ; i < 3; i++){
        tmp = _abs(paccel[i] - test_last_accel[i]);
        if (tmp > TEST_ACCEL_VAL){
            test_bias = TEST_COUNT+1;
            goto _MPU6050_recv_FIFO_con;
        }
        test_accel[i] += paccel[i];
        pbuf+=2;

    }
    zflag = paccel[2] >= 0;
    
    test_accel[2] +=  TEST_MPU6050_1G_VAL[zflag];
    test_bias++;
    if (test_bias >= TEST_COUNT){
        tmp = 0;
        for (i = 0 ; i < 3 ; i++){
            test_accel[i] = ((test_accel[i] +((int16_t)1<<(MPU6050_ACCEL_SHF-1)))>> MPU6050_ACCEL_SHF);
            tmp += _abs(test_accel[i]);
        }
        if (tmp >= 3){
            adjust_accel_user_offset(test_accel);
            Serial_printStrN(test_accel[0]);
            Serial_printStrN(test_accel[1]);
            Serial_printStrN(test_accel[2]);
        }
        
_MPU6050_recv_FIFO_clean:
        test_bias = 0;
        test_accel[0] = test_accel[1] = test_accel[2] = 0;
    }
_MPU6050_recv_FIFO_con:
    for (i = 0 ; i< 3 ; i++){
        test_last_accel[i] = paccel[i];
    }
    
#endif
    recv_FIFO = 2;
}

上面adjust_accel_user_offset(test_accel);测试的约1200us。如果你和我一样是准实时操作,每个时间片(1个ms),执行完当前各种动作,那么这个函数需要拆多步骤完成。调用该函数的时候,其实可以根据当前积分的速度大小来判定,是否对积分出来的速度进行清零操作。

各种宏就不给了,完全是自己的一套东西,给了也没用。仅供参考。哈。


最后给出一组累计速度的数据。下面各列依次是,序号,x,y,z三轴通过加速度累计的当前速度,x轴当前加速度值(单位0.000488g),前序采样64个的平均值,随后是y轴,z轴各两列,最后一列是每次采样的时间间隔(单位毫秒),精度是4个us。


X,Y,Z轴的速度,在18秒内(基本每隔10ms打一行),仍然稳定在0.0627和0.0649和0.101左右,反正是正负漂,无所谓,至于启动时的一些累计误差,这很容易滤掉。在前面讨论中说过,调用adjust_accel_user_offset(test_accel);时,把速度清0,于是就有了下面的效果。哈。


在2283行,发生过一次自校验。。

哦。补充说一句,以上是在16MHz的atmega328p上跑的数据。arm暂时不碰,自然dsp不动。。。哈。


加载中
0
54mark
54mark
哈,不懂
0
明月惊鹊
明月惊鹊
哈, 好高深的样子
0
六月是你的谎言
六月是你的谎言
所以,mpu6050到底是什么。。。
0
改着名儿玩
改着名儿玩
这什么??
0
陈阳阳阳
陈阳阳阳
道理我都懂,可是MPU6050是什么
0
大梦1107
大梦1107

buf_cpy(pbuf,data,length);你这容易溢出啊

0
羊驼君
羊驼君
大概了解了一下,MPU6050陀螺仪组件,模拟控制,例如无人机等等。。。
0
中山野鬼
中山野鬼

引用来自“abcdefghig”的评论

buf_cpy(pbuf,data,length);你这容易溢出啊

还好还好,单片机的东西,前后两个buf的和length其实都是固定的。。无非接口按照传统方式来实现,以替代memcpy。。。
返回顶部
顶部