MWC_computeIMU算法解析(修改)20130908課件

1、 對(duì)MWC重要函數(shù)的理解目錄一、原文件的理解二、我對(duì)main loop()主程序的分析三、conputeIMU函數(shù)以及它調(diào)用的估計(jì)姿態(tài)函數(shù)getEstimatedAttitude()函數(shù)的分析四、旋轉(zhuǎn)矩陣求坐標(biāo)解析五、計(jì)算俯仰角度的依據(jù)文件一、原文件的理解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（一種硬件設(shè)備）執(zhí)行下面的 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();/獲得加計(jì)的ADC值 getEstimatedAttitude();/獲取估計(jì)

6、姿態(tài) #endif #if GYRO Gyro_getADC(); #endif for (axis = 0; axis < 3; axis+) gyroADCpaxis = gyroADCaxis; timeInterleave=micros(); annexCode();/通過串口向電腦或GUI發(fā)送MWC的實(shí)時(shí)數(shù)據(jù)。 if (micros()-timeInterleave)>650) annex650_overrun_count+;/運(yùn)行時(shí)間超時(shí) 并進(jìn)行計(jì)數(shù) else while(micros()-timeInterleave)<650) ; /empirical, in

7、terleaving delay between 2 consecutive reads /當(dāng)運(yùn)行時(shí)間不足650ms時(shí) 依據(jù)驗(yàn)證 進(jìn)行2次連續(xù)的讀內(nèi)部延時(shí) #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/通過試驗(yàn)獲得前面的值和當(dāng)前值的權(quán)重 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;/靜態(tài)變量設(shè)置 第一次運(yùn)行時(shí)進(jìn)行初始化，以后保留前次值再更新。 for (axis = 0; axis < 3; axis+) gyroDataaxis = (int16_t) ( ( (int32_t)(int32_t

9、)gyroSmoothaxis * (conf.Smoothingaxis-1) )+gyroDataaxis+1 ) / conf.Smoothingaxis);/陀螺儀數(shù)據(jù)平滑濾波 gyroSmoothaxis = gyroDataaxis; #elif defined(TRI)/假如定義為三腳機(jī) static int16_t gyroYawSmooth = 0; gyroDataYAW = (gyroYawSmooth*2+gyroDataYAW)/3; gyroYawSmooth = gyroDataYAW; #endif/ */ Simplified IMU based on &qu

10、ot;Complementary Filter"/基于互補(bǔ)濾波簡(jiǎn)化的IMU計(jì)算/ Inspired by 創(chuàng)新來自于/ adapted by 改編來自于ziss_dm : / The following ideas was used in this project:/后面的網(wǎng)站知識(shí)將要用在這個(gè)工程里面/ 1) Rotation matrix旋轉(zhuǎn)矩陣: /wiki/Rotation_matrix/ 2) Small-angle approximation小角度近似算法 : /wiki/Small-a

11、ngle_approximation/ 3) C. Hastings approximation for atan2() /antan2的近似算法/ 4) Optimization tricks優(yōu)化: / Currently Magnetometer uses separate CF which is used only/ for heading approximation./磁力計(jì) 用于近似定向/ */* advanced users settings */高級(jí)用戶設(shè)置/* Set the Low Pass Filter factor

12、for ACC */設(shè)置加速度計(jì)的低通濾波因子/* 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 */設(shè)置磁力

13、計(jì)的低通濾波因子/* 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 */設(shè)置陀螺儀權(quán)重在加速

14、度計(jì)值與加速度互補(bǔ)濾波值的權(quán)重因子/* 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 */設(shè)置陀螺儀權(quán)重在加速度計(jì)值與磁力計(jì)互補(bǔ)濾波值的權(quán)重因子/* 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 */結(jié)束高級(jí)用戶設(shè)置#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/根據(jù)陀螺儀的角度值使用旋轉(zhuǎn)矩陣的小角度近似算法 該函數(shù)每個(gè)循環(huán)2次被調(diào)用，其中一次的調(diào)用是求得

20、上次加速度值延時(shí)一段時(shí)間后新的一個(gè)估計(jì)值（修證值）這個(gè)值在后面的計(jì)算中權(quán)重400，而新獲取濾波后的加速度值的權(quán)重只有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軸數(shù)據(jù)的基礎(chǔ)上，到這個(gè)時(shí)候已經(jīng)有時(shí)間延時(shí)，也發(fā)生了角度的變化，通過旋轉(zhuǎn)得到了一個(gè)前次基礎(chǔ)上估計(jì)的加速度向量 #if MAG rotateV(&EstM.V,deltaGyroAngle); /在上次獲得電子羅盤計(jì)3軸數(shù)據(jù)的基礎(chǔ)上，到這個(gè)時(shí)候已經(jīng)有時(shí)間延時(shí)，也發(fā)生

26、了角度的變化，通過旋轉(zhuǎn)得到了一個(gè)前次基礎(chǔ)上估計(jì)的電子羅盤計(jì)向量 #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;/以前數(shù)據(jù)基礎(chǔ)上估計(jì)的加速度計(jì)數(shù)據(jù)（就是旋轉(zhuǎn)矩陣運(yùn)算后的數(shù)據(jù)）占400份，而本次循環(huán)取得的acc數(shù)據(jù)只占1份 總和是401份除以401就得到了歷史上的數(shù)據(jù)和最新數(shù)據(jù)的融合與濾波。 #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) ;. /得到滾轉(zhuǎn)的角度 見后面的依據(jù)文件 anglePITCH = _atan2(EstG.V.Y , EstG.V.Z) ; /得到俯仰的角度 見后面的依據(jù)文件 #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 );/得到四軸的定向角（為何這么計(jì)算還不知道，有知道的QQXXXXXXXXX ） heading += MAG_DECLINIATION * 10; /add declination/因磁偏角原因進(jìn)行修正 heading = heading /10; if ( heading > 180) heading = heading - 360; else if (heading < -180) heading = heading + 360; #endif二、我對(duì)main loop()主程序的分析運(yùn)行開始段代碼MWC飛控的自檢進(jìn)行設(shè)置，通過電腦或遙控桿進(jìn)

31、行MWC飛控的設(shè)置。調(diào)用讀磁力計(jì)Mag_getADC(); 讀氣壓計(jì)（獲得估計(jì)高度） 讀GPS 讀聲納調(diào)用computeIMU ()的理解（見下面一頁）取得加速度計(jì)的三軸值 ACC_getADC();/獲得加速度計(jì)的ADC值運(yùn)行g(shù)etEstimatedAttitude()函數(shù)的時(shí)序取得陀螺儀的三軸值 Gyro_getADC();/獲得陀螺儀的ADC值 運(yùn)行其它代碼磁力計(jì)定向程序運(yùn)行 氣壓定高程序運(yùn)行GPS回家功能或GPS定點(diǎn)運(yùn)行程序PITCH ROLL YAW的PID調(diào)整 融合數(shù)據(jù) 及給舵機(jī)、馬達(dá)發(fā)送指令三、conputeIMU函數(shù)以及它調(diào)用的估計(jì)姿態(tài)函數(shù)getEstimatedAttitud

32、e()函數(shù)的分析取、留時(shí)間用于陀螺儀積分，獲得角度，這是個(gè)關(guān)鍵點(diǎn)。運(yùn)行設(shè)置 自檢程序 。調(diào)用Mag_getADC();。調(diào)用computeIMU ()的理解。取得加速度計(jì)的三軸值 ACC_getADC();/獲得加速度計(jì)的ADC值。運(yùn)行g(shù)etEstimatedAttitude()函數(shù)的時(shí)序。獲取時(shí)間段（此時(shí)的時(shí)間以前存儲(chǔ)時(shí)間）。記下此次時(shí)間作為下次需要的存儲(chǔ)時(shí)間。獲得陀螺儀的三軸值（時(shí)間段*角速度）。本循環(huán)前次的加速度計(jì)值因?yàn)橐呀?jīng)發(fā)生了運(yùn)行體角度變化。本循環(huán)前次的電子羅盤計(jì)值因?yàn)橐呀?jīng)發(fā)生了運(yùn)行體角度變化。故使用旋轉(zhuǎn)函數(shù)調(diào)整rotateV(struct fp_vector *v,float* d

33、elta)目的是。獲取原來數(shù)據(jù)基礎(chǔ)上的一個(gè)帶有歷史記憶的一份推測(cè)值加速度3軸向量。獲取原來數(shù)據(jù)基礎(chǔ)上的一個(gè)帶有歷史記憶的一份推測(cè)值電子羅盤計(jì)的3軸向量。(將帶有歷史記憶推測(cè)出的加速度計(jì)值*400最新加速度值*1)/401進(jìn)行濾波計(jì)算出一個(gè)可認(rèn)可的一份3軸加速度值。(將帶有歷史記憶推測(cè)出的羅盤計(jì)值*400最新羅盤計(jì)值*1)/401進(jìn)行濾波計(jì)算出一個(gè)可認(rèn)可的一份3軸羅盤計(jì)值 。依據(jù)計(jì)算出來的可認(rèn)可的加速度值計(jì)算出俯仰角和旋轉(zhuǎn)角 。依據(jù)濾波計(jì)算出來的加速度3軸值和羅盤3軸值計(jì)算出航向角。取得陀螺儀的三軸值 Gyro_getADC();/獲得陀螺儀的ADC值 下次循環(huán)將使用到。運(yùn)行其它代碼 開始循環(huán)（

34、取陀螺儀函數(shù)是在積分計(jì)算時(shí)間開始到結(jié)束的中間段調(diào)用獲取值的） 四、旋轉(zhuǎn)矩陣求坐標(biāo)解析（一）、原程序void rotateV(struct fp_vector *v,float* delta) fp_vector v_tmp = *v;/拷貝一個(gè)副本到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，代碼簡(jiǎn)寫成下行代碼/ v->Z =v_tmp.Z- deltaROLL * v_tmp.X - deltaPITCH * v_tmp.Y;這就是寫出矩陣的原