MWC_computeIMU算法解析(修改)20130908

1、 对MWC重要函数的理解目录一、原文件的理解二、我对main loop()主程序的分析三、conputeIMU函数以及它调用的估计姿态函数getEstimatedAttitude()函数的分析四、旋转矩阵求坐标解析五、计算俯仰角度的依据文件一、原文件的理解void computeIMU () uint8_t axis; static int16_t gyroADCprevious3 = 0,0,0; int16_t gyroADCp3; int16_t gyroADCinter3; static uint32_t timeInterleave = 0; /we separate the 2 s

2、ituations because reading gyro values with a gyro only setup can be acchieved at a higher rate /gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms /gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms #if d

3、efined(NUNCHUCK)/定义NUNCHUCK（一种硬件设备）执行下面的 annexCode(); while(micros()-timeInterleave)<INTERLEAVING_DELAY) ; /interleaving delay between 2 consecutive reads timeInterleave=micros(); ACC_getADC(); getEstimatedAttitude(); / computation time must last less than one interleaving delay while(micros()-ti

4、meInterleave)<INTERLEAVING_DELAY) ; /interleaving delay between 2 consecutive reads timeInterleave=micros(); f.NUNCHUKDATA = 1; while(f.NUNCHUKDATA) ACC_getADC(); / For this interleaving reading, we must have a gyro update at this point (less delay) for (axis = 0; axis < 3; axis+) / empirical,

5、 we take a weighted value of the current and the previous values / /4 is to average 4 values, note: overflow is not possible for WMP gyro here gyroDataaxis = (gyroADCaxis*3+gyroADCpreviousaxis)/4; gyroADCpreviousaxis = gyroADCaxis; #else #if ACC/. ACC_getADC();/获得加计的ADC值 getEstimatedAttitude();/获取估计

6、姿态 #endif #if GYRO Gyro_getADC(); #endif for (axis = 0; axis < 3; axis+) gyroADCpaxis = gyroADCaxis; timeInterleave=micros(); annexCode();/通过串口向电脑或GUI发送MWC的实时数据。 if (micros()-timeInterleave)>650) annex650_overrun_count+;/运行时间超时 并进行计数 else while(micros()-timeInterleave)<650) ; /empirical, in

7、terleaving delay between 2 consecutive reads /当运行时间不足650ms时 依据验证 进行2次连续的读内部延时 #if GYRO Gyro_getADC(); #endif for (axis = 0; axis < 3; axis+) /陀螺仪的值求平均 gyroADCinteraxis = gyroADCaxis+gyroADCpaxis; / empirical, we take a weighted value of the current and the previous values/通过试验获得前面的值和当前值的权重 gyroDa

8、taaxis = (gyroADCinteraxis+gyroADCpreviousaxis)/3; gyroADCpreviousaxis = gyroADCinteraxis/2; if (!ACC) accADCaxis=0; #endif #if defined(GYRO_SMOOTHING) static int16_t gyroSmooth3 = 0,0,0;/静态变量设置 第一次运行时进行初始化，以后保留前次值再更新。 for (axis = 0; axis < 3; axis+) gyroDataaxis = (int16_t) ( ( (int32_t)(int32_t

9、)gyroSmoothaxis * (conf.Smoothingaxis-1) )+gyroDataaxis+1 ) / conf.Smoothingaxis);/陀螺仪数据平滑滤波 gyroSmoothaxis = gyroDataaxis; #elif defined(TRI)/假如定义为三脚机 static int16_t gyroYawSmooth = 0; gyroDataYAW = (gyroYawSmooth*2+gyroDataYAW)/3; gyroYawSmooth = gyroDataYAW; #endif/ */ Simplified IMU based on &qu

10、ot;Complementary Filter"/基于互补滤波简化的IMU计算/ Inspired by 创新来自于/ adapted by 改编来自于ziss_dm : / The following ideas was used in this project:/后面的网站知识将要用在这个工程里面/ 1) Rotation matrix旋转矩阵: /wiki/Rotation_matrix/ 2) Small-angle approximation小角度近似算法 : /wiki/Small-a

11、ngle_approximation/ 3) C. Hastings approximation for atan2() /antan2的近似算法/ 4) Optimization tricks优化: / Currently Magnetometer uses separate CF which is used only/ for heading approximation./磁力计 用于近似定向/ */* advanced users settings */高级用户设置/* Set the Low Pass Filter factor

12、for ACC */设置加速度计的低通滤波因子/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/* Comment this if you do not want filter at all.*/注释掉这句代码，假如你不需要滤波#ifndef ACC_LPF_FACTOR #define ACC_LPF_FACTOR 100#endif/* Set the Low Pass Filter factor for Magnetometer */设置磁力

13、计的低通滤波因子/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/* Comment this if you do not want filter at all.*/#ifndef MG_LPF_FACTOR /#define MG_LPF_FACTOR 4#endif/* Set the Gyro Weight for Gyro/Acc complementary filter */设置陀螺仪权重在加速

14、度计值与加速度互补滤波值的权重因子/* Increasing this value would reduce and delay Acc influence on the output of the filter*/#ifndef GYR_CMPF_FACTOR #define GYR_CMPF_FACTOR 400.0f#endif/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */设置陀螺仪权重在加速度计值与磁力计互补滤波值的权重因子/* Increasing this value would reduce

15、 and delay Magnetometer influence on the output of the filter*/#ifndef GYR_CMPFM_FACTOR #define GYR_CMPFM_FACTOR 200.0f#endif/* end of advanced users settings */结束高级用户设置#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f)#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f)#if GYRO

16、#define GYRO_SCALE (2380 * PI)/(32767.0f / 4.0f ) * 180.0f * 1000000.0f) /should be 2279.44 but 2380 gives better result / +-2000/sec deg scale /#define GYRO_SCALE (200.0f * PI)/(32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f) / +- 200/sec deg scale / 1.5 is emperical, not sure what it means

17、 / should be in rad/sec#else #define GYRO_SCALE (1.0f/200e6f) / empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility / !should be adjusted to the rad/sec#endif / Small angle approximation#define ssin(val) (val)#define scos(val) 1.0ftypedef struct fp_vector float X; float Y; float Z

18、; t_fp_vector_def;typedef union float A3; t_fp_vector_def V; t_fp_vector;int16_t _atan2(float y, float x) #define fp_is_neg(val) (uint8_t*)&val)3 & 0x80) != 0) float z = y / x; int16_t zi = abs(int16_t(z * 100); int8_t y_neg = fp_is_neg(y); if ( zi < 100 ) if (zi > 10) z = z / (1.0f +

19、0.28f * z * z); if (fp_is_neg(x) if (y_neg) z -= PI; else z += PI; else z = (PI / 2.0f) - z / (z * z + 0.28f); if (y_neg) z -= PI; z *= (180.0f / PI * 10); return z;/ Rotate Estimated vector(s) with small angle approximation, according to the gyro data/根据陀螺仪的角度值使用旋转矩阵的小角度近似算法 该函数每个循环2次被调用，其中一次的调用是求得

20、上次加速度值延时一段时间后新的一个估计值（修证值）这个值在后面的计算中权重400，而新获取滤波后的加速度值的权重只有1。void rotateV(struct fp_vector *v,float* delta) . fp_vector v_tmp = *v; v->Z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y; v->X += deltaROLL * v_tmp.Z - deltaYAW * v_tmp.Y; v->Y += deltaPITCH * v_tmp.Z + deltaYAW * v_tmp.X; #define

21、 ACC_LPF_FOR_VELOCITY 10static float accLPFVel3;static t_fp_vector EstG;void getEstimatedAttitude() uint8_t axis; int32_t accMag = 0;#if MAG static t_fp_vector EstM;#endif#if defined(MG_LPF_FACTOR) static int16_t mgSmooth3; #endif#if defined(ACC_LPF_FACTOR) static float accLPF3;#endif static uint16_

22、t previousT; uint16_t currentT = micros(); float scale, deltaGyroAngle3; scale = (currentT - previousT) * GYRO_SCALE; previousT = currentT; / Initialization for (axis = 0; axis < 3; axis+) /修整平滑滤波3轴陀螺仪值 和3轴加速度值 deltaGyroAngleaxis = gyroADCaxis * scale; #if defined(ACC_LPF_FACTOR) accLPFaxis = acc

23、LPFaxis * (1.0f - (1.0f/ACC_LPF_FACTOR) + accADCaxis * (1.0f/ACC_LPF_FACTOR); accLPFVelaxis = accLPFVelaxis * (1.0f - (1.0f/ACC_LPF_FOR_VELOCITY) + accADCaxis * (1.0f/ACC_LPF_FOR_VELOCITY); accSmoothaxis = accLPFaxis; #define ACC_VALUE accSmoothaxis #else accSmoothaxis = accADCaxis; #define ACC_VALU

24、E accADCaxis #endif/ accMag += (ACC_VALUE * 10 / (int16_t)acc_1G) * (ACC_VALUE * 10 / (int16_t)acc_1G); accMag += (int32_t)ACC_VALUE*ACC_VALUE ; #if MAG #if defined(MG_LPF_FACTOR) mgSmoothaxis = (mgSmoothaxis * (MG_LPF_FACTOR - 1) + magADCaxis) / MG_LPF_FACTOR; / LPF for Magnetometer values #define

25、MAG_VALUE mgSmoothaxis #else #define MAG_VALUE magADCaxis #endif #endif accMag = accMag*100/(int32_t)acc_1G*acc_1G); rotateV(&EstG.V,deltaGyroAngle);. /在上次获得加速度3轴数据的基础上，到这个时候已经有时间延时，也发生了角度的变化，通过旋转得到了一个前次基础上估计的加速度向量 #if MAG rotateV(&EstM.V,deltaGyroAngle); /在上次获得电子罗盘计3轴数据的基础上，到这个时候已经有时间延时，也发生

26、了角度的变化，通过旋转得到了一个前次基础上估计的电子罗盘计向量 #endif if ( abs(accSmoothROLL)<acc_25deg && abs(accSmoothPITCH)<acc_25deg && accSmoothYAW>0) f.SMALL_ANGLES_25 = 1; else f.SMALL_ANGLES_25 = 0; / Apply complimentary filter (Gyro drift correction) / If accel magnitude >1.4G or <0.6G and

27、ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. / To do that, we just skip filter, as EstV already rotated by Gyro if ( ( 36 < accMag && accMag < 196 ) | f.SMALL_ANGLES_25 ) for (axis = 0; axis < 3; axis+) int16_t acc =

28、 ACC_VALUE; EstG.Aaxis = (EstG.Aaxis * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;/以前数据基础上估计的加速度计数据（就是旋转矩阵运算后的数据）占400份，而本次循环取得的acc数据只占1份 总和是401份除以401就得到了历史上的数据和最新数据的融合与滤波。 #if MAG for (axis = 0; axis < 3; axis+) EstM.Aaxis = (EstM.Aaxis * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR; #e

29、ndif / Attitude of the estimated vector angleROLL = _atan2(EstG.V.X , EstG.V.Z) ;. /得到滚转的角度 见后面的依据文件 anglePITCH = _atan2(EstG.V.Y , EstG.V.Z) ; /得到俯仰的角度 见后面的依据文件 #if MAG / Attitude of the cross product vector GxM heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - Est

30、G.V.Y * EstM.V.Z );/得到四轴的定向角（为何这么计算还不知道，有知道的QQXXXXXXXXX ） heading += MAG_DECLINIATION * 10; /add declination/因磁偏角原因进行修正 heading = heading /10; if ( heading > 180) heading = heading - 360; else if (heading < -180) heading = heading + 360; #endif二、我对main loop()主程序的分析运行开始段代码MWC飞控的自检进行设置，通过电脑或遥控杆进

31、行MWC飞控的设置。调用读磁力计Mag_getADC(); 读气压计（获得估计高度） 读GPS 读声纳调用computeIMU ()的理解（见下面一页）取得加速度计的三轴值 ACC_getADC();/获得加速度计的ADC值运行getEstimatedAttitude()函数的时序取得陀螺仪的三轴值 Gyro_getADC();/获得陀螺仪的ADC值 运行其它代码磁力计定向程序运行 气压定高程序运行GPS回家功能或GPS定点运行程序PITCH ROLL YAW的PID调整 融合数据 及给舵机、马达发送指令三、conputeIMU函数以及它调用的估计姿态函数getEstimatedAttitud

32、e()函数的分析取、留时间用于陀螺仪积分，获得角度，这是个关键点。运行设置 自检程序 。调用Mag_getADC();。调用computeIMU ()的理解。取得加速度计的三轴值 ACC_getADC();/获得加速度计的ADC值。运行getEstimatedAttitude()函数的时序。获取时间段（此时的时间以前存储时间）。记下此次时间作为下次需要的存储时间。获得陀螺仪的三轴值（时间段*角速度）。本循环前次的加速度计值因为已经发生了运行体角度变化。本循环前次的电子罗盘计值因为已经发生了运行体角度变化。故使用旋转函数调整rotateV(struct fp_vector *v,float* d

33、elta)目的是。获取原来数据基础上的一个带有历史记忆的一份推测值加速度3轴向量。获取原来数据基础上的一个带有历史记忆的一份推测值电子罗盘计的3轴向量。(将带有历史记忆推测出的加速度计值*400最新加速度值*1)/401进行滤波计算出一个可认可的一份3轴加速度值。(将带有历史记忆推测出的罗盘计值*400最新罗盘计值*1)/401进行滤波计算出一个可认可的一份3轴罗盘计值 。依据计算出来的可认可的加速度值计算出俯仰角和旋转角 。依据滤波计算出来的加速度3轴值和罗盘3轴值计算出航向角。取得陀螺仪的三轴值 Gyro_getADC();/获得陀螺仪的ADC值 下次循环将使用到。运行其它代码 开始循环（

34、取陀螺仪函数是在积分计算时间开始到结束的中间段调用获取值的） 四、旋转矩阵求坐标解析（一）、原程序void rotateV(struct fp_vector *v,float* delta) fp_vector v_tmp = *v;/拷贝一个副本到v_tmp中。 v->Z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y;/v->Z -= deltaROLL * v_tmp.X + deltaPITCH * v_tmp.Y;改写成下行代码/ v->Z = v->Z - deltaROLL * v_tmp.X - deltaPITCH * v_tmp.Y; 利用v_tmp.Z=V->Z，代码简写成下行代码/ v->Z =v_tmp.Z- deltaROLL * v_tmp.X - deltaPITCH * v_tmp.Y;这就是写出矩阵的原