|
|
楼主 |
发表于 2014-3-25 09:24:13
|
显示全部楼层
灵敏度调整只需要在初始化的时候做一次就行了，Hadj=H*((ASA-128)/256+1)自己化简一下：Hadj=H*((ASA+128)/256)，移位比除法快，换成代码就是H*((ASA+128)>>8)，结果就是mag=(mag*(ASA+128))>>8
mag的机制就是每读取一次MXL到MZH，mag都会从single measure转到power down mode，所以每次读取完数据，要执行一次single measure，然后在下一次读取数据之前，先读取一下ST1这个寄存器，看数据是否准备好，if没准备好就不读取，准备好了就读取，这样可以大幅度提高姿态解算的频率，没必要死等mag数据，毕竟mag本身没那么高的output rate，也不需要很快的得到它，基本保证10ms读一次mag就够了
代码顺序给你：
- i2cread(Compass_Addr, Compass_ST1, 1, tmp+6);
- if(tmp[6] == 1)
- {
- i2cread(Compass_Addr, Compass_HXL, 6, tmp);//读取compass
- 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_sens_adj_val=ASA+128
- mag[1] = ((long)mag[1] * mag_sens_adj_val[1]) >> 8;
- mag[2] = ((long)mag[2] * mag_sens_adj_val[2]) >> 8;
- //修正mag
- init_mx =(float)mag[1] * MXgain + MXoffset; //转换坐标轴
- init_my =(float)mag[0] * MYgain + MYoffset;
- init_mz =(float)-mag[2] * MZgain + MZoffset;
- i2cwrite(Compass_Addr, Compass_CNTL, 1, data_write+6); //开启compass：single measurement mode
- }
最近姿态解算和PID都完成了，稍后会放上完整代码。。。 |
|
楼主: zksniper
[复制链接]
发表于 2013-12-9 22:05:52
，谢谢指点！
