|
![](static/image/common/ico_lz.png)
楼主 |
发表于 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都完成了,稍后会放上完整代码。。。 |
|