接上篇。
IMU主要分为3大部分：
1. ?computeIMU ()：提供给外的接口函数，也是传感器处理的总函数；
2.?getEstimatedAttitude（）：获取估算的姿态，主要处理ACC、Gyro和Mag传感器数据；
3.getEstimatedAltitude（）：获取估算的高度，主要处理Baro传感器数据。
黑色为保留的代码，红色为可删除的多余代码。
/***************************************************************************************/
//1.
void computeIMU () {
? uint8_t axis;
? static int16_t gyroADCprevious[3] = {0,0};
? int16_t gyroADCp[3];
? int16_t gyroADCinter[3];
? static int16_t lastAccADC[3] = {0,0};
? static uint32_t timeInterleave = 0;
? static int16_t gyroYawSmooth = 0;
? //we separate the 2 situations 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 (!ACC && nunchuk) {
? ? annexCode();
? ? while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
? ? timeInterleave=micros();
? ? WMP_getRawADC();
? ? getEstimatedAttitude(); // computation time must last less than one interleaving delay
? ? #if BARO
? ? ? getEstimatedAltitude();
? ? #endif?
? ? while((micros()-timeInterleave)<INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
? ? timeInterleave=micros();
? ? while(WMP_getRawADC() != 1) ; // For this interleaving reading,we must have a gyro update at this point (less delay)
? ? for (axis = 0; axis < 3; axis++) {
? ? ? // empirical,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
? ? ? gyroData[axis] = (gyroADC[axis]*3+gyroADCprevious[axis]+2)/4;
? ? ? gyroADCprevious[axis] = gyroADC[axis];
? ? }
? } else {
? ? if (ACC) {
? ? ? ACC_getADC(); ? ? ? ? ? ? ? ? ? //有加速度传感器
? ? ? getEstimatedAttitude(); ? ? //估算姿态
? ? ? if (BARO) getEstimatedAltitude(); ? //有气压传感器，估算高度
? ? }
? ? if (GYRO) Gyro_getADC(); else WMP_getRawADC(); ?
? ? for (axis = 0; axis < 3; axis++)
? ? ? gyroADCp[axis] = ?gyroADC[axis]; ? ?//将从Sensors传来的数据保存
? ? timeInterleave=micros();
? ? annexCode(); ? ? ? //此函数代码后续再做探讨
? ? while((micros()-timeInterleave)<650) ; //empirical,interleaving delay between 2 consecutive reads ? ?//给annexCode(); 运行时间
? ? if (GYRO) Gyro_getADC(); else WMP_getRawADC();
? ? for (axis = 0; axis < 3; axis++) {
? ? ? gyroADCinter[axis] = ?gyroADC[axis]+gyroADCp[axis];
? ? ? // empirical,we take a weighted value of the current and the previous values
? ? ? gyroData[axis] = (gyroADCinter[axis]+gyroADCprevious[axis]+1)/3; ? ? ? //对最近三次的Gyro数据取平均值
? ? ? gyroADCprevious[axis] = gyroADCinter[axis]/2;
? ? ? if (!ACC) accADC[axis]=0;
? ? }
? }
? #if defined(TRI)
? ? gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW]+1)/3;
? ? gyroYawSmooth = gyroData[YAW];
? #endif
}
/***************************************************************************************/
//2
// **************************************************
// Simplified IMU based on "Complementary Filter"
// Inspired by http://starlino.com/imu_guide.html
//
// adapted by ziss_dm : http://wbb.multiwii.com/viewtopic.php?f=8&t=198
//
// The following ideas was used in this project:
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
// 3) C. Hastings approximation for atan2()
// 4) Optimization tricks: http://www.hackersdelight.org/
//
// Currently Magnetometer uses separate CF which is used only
// for heading approximation.
//
// Last Modified: 19/04/2011
// Version: V1.1 ??
// **************************************************
//****** ?advanced users settings *******************
/* Set the Low Pass Filter factor 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.*/
/* Default WMC value: 8*/
#define ACC_LPF_FACTOR 8
/* Set the Low Pass Filter factor for Magnetometer */
/* 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.*/
/* Default WMC value: n/a*/
//#define MG_LPF_FACTOR 4
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
/* Default WMC value: 300*/
#define GYR_CMPF_FACTOR 310.0f
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
/* Default WMC value: n/a*/
#define GYR_CMPFM_FACTOR 200.0f
//****** 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
? #define GYRO_SCALE ((2000.0f * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.155f) ?
? // +-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
? // 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.0f
typedef struct {
? float X;
? float Y;
? float Z;
} t_fp_vector_def;
typedef union {
? float ? A[3];
? t_fp_vector_def V;
} t_fp_vector;
int16_t _atan2(float y,float x){ ? ? ? ? ? //坐标（x,y）与x轴形成的角度
? #define fp_is_neg(val) ((((byte*)&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 + 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;
}
void getEstimatedAttitude(){ ? ?//估计姿态
? uint8_t axis;
? uint16_t accLim,accMag = 0;
? static t_fp_vector EstG = {0,300};
? static t_fp_vector EstM = {0,300};
? float scale,deltaGyroAngle;
? static int16_t mgSmooth[3]; ?//projection of smoothed and normalized magnetic vector on x/y/z axis,as measured by magnetometer
? static uint16_t previousT;
? uint16_t currentT;
??
? currentT ?= micros();
? scale = (currentT - previousT) * GYRO_SCALE;
? previousT = currentT;
??
? // Initialization
? for (axis = 0; axis < 3; axis++) {
? ? #if defined(ACC_LPF_FACTOR)
// ? ? ?accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR; // LPF for ACC values
? ? ? ? accSmooth[axis] =(accSmooth[axis]*7+accADC[axis]+4)>>3;
? ? ? #define ACC_VALUE accSmooth[axis]
? ? #else ?
? ? ? accSmooth[axis] = accADC[axis]; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //对ACC数据进行滤波
? ? ? #define ACC_VALUE accADC[axis] ? ? ? ? ? ? ? ? ? ? ? ?
? ? #endif
? ? accMag += (ACC_VALUE * 10 / acc_1G) * (ACC_VALUE * 10 / acc_1G); //788 ? ? ? ? ? ? ? // ?加速度的数量级 ? ax*ax+ay*ay+az*az
? ? #if MAG ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?//对MAG进行滤波处理
? ? ? #if defined(MG_LPF_FACTOR)
? ? ? ? mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
? ? ? ? #define MAG_VALUE mgSmooth[axis]
? ? ? #else ?
? ? ? ? #define MAG_VALUE magADC[axis]
? ? ? #endif
? ? #endif ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
? }
//根据Gyro数据算出angle[Roll] 和 angle[Pitch]
? // Rotate Estimated vector(s),ROLL
? deltaGyroAngle ?= gyroADC[ROLL] * scale;
? EstG.V.Z = ?scos(deltaGyroAngle) * EstG.V.Z - ssin(deltaGyroAngle) * EstG.V.X;
? EstG.V.X = ?ssin(deltaGyroAngle) * EstG.V.Z + scos(deltaGyroAngle) * EstG.V.X;
? #if MAG
? ? EstM.V.Z = ?scos(deltaGyroAngle) * EstM.V.Z - ssin(deltaGyroAngle) * EstM.V.X;
? ? EstM.V.X = ?ssin(deltaGyroAngle) * EstM.V.Z + scos(deltaGyroAngle) * EstM.V.X;
? #endif?
? // Rotate Estimated vector(s),PITCH
? deltaGyroAngle ?= gyroADC[PITCH] * scale;
? EstG.V.Y = ?scos(deltaGyroAngle) * EstG.V.Y + ssin(deltaGyroAngle) * EstG.V.Z;
? EstG.V.Z = -ssin(deltaGyroAngle) * EstG.V.Y + scos(deltaGyroAngle) * EstG.V.Z;
? #if MAG
? ? EstM.V.Y = ?scos(deltaGyroAngle) * EstM.V.Y + ssin(deltaGyroAngle) * EstM.V.Z;
? ? EstM.V.Z = -ssin(deltaGyroAngle) * EstM.V.Y + scos(deltaGyroAngle) * EstM.V.Z;
? #endif?
? // Rotate Estimated vector(s),YAW
? deltaGyroAngle ?= gyroADC[YAW] * scale;
? EstG.V.X = ?scos(deltaGyroAngle) * EstG.V.X - ssin(deltaGyroAngle) * EstG.V.Y;
? EstG.V.Y = ?ssin(deltaGyroAngle) * EstG.V.X + scos(deltaGyroAngle) * EstG.V.Y;
? #if MAG
? ? EstM.V.X = ?scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;
? ? EstM.V.Y = ?ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;
? #endif
?
//滤波，当加速度数量级不满足要求时候就舍弃数据
? // Apply complimentary filter (Gyro drift correction)
? // If accel magnitude >1.4G or <0.6G and 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
? accLim = acc_1G*4/10;
? if ( ( 36 < accMag && accMag < 196 ) || ( ?abs(accSmooth[ROLL])<accLim ?&& abs(accSmooth[PITCH])<accLim ) ) {
? ? for (axis = 0; axis < 3; axis++)
? ? ? EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;
? }
? // Attitude of the estimated vector
? angle[ROLL] ?= ?_atan2(EstG.V.X,EstG.V.Z) ;
? angle[PITCH] = ?_atan2(EstG.V.Y,EstG.V.Z) ;
? #if MAG ? ? ? ? ? ? ?//对磁场数据进行处理，定向功能
? ? // Apply complimentary filter (Gyro drift correction)
? ? for (axis = 0; axis < 3; axis++)
? ? ? EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR - MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
? ? // Attitude of the cross product vector GxM
? ? heading = _atan2(EstG.V.Z * EstM.V.X - EstG.V.X * EstM.V.Z,EstG.V.Y * EstM.V.Z - EstG.V.Z * EstM.V.Y) / 10;
? #endif
}
/***************************************************************************************/
//3.
float InvSqrt (float x){?
? union{ ?
? ? int32_t i; ?
? ? float ? f;?
? } conv;?
? conv.f = x;?
? conv.i = 0x5f3759df - (conv.i >> 1);?
? return 0.5f * conv.f * (3.0f - x * conv.f * conv.f);
} ?
int32_t isq(int32_t x){return x * x;}
#define UPDATE_INTERVAL 25000 ? ?// 40hz update rate (20hz LPF on acc)
#define INIT_DELAY ? ? ?4000000 ?// 4 sec initialization delay
#define Kp1 0.55f ? ? ? ? ? ? ? ?// PI observer velocity gain?
#define Kp2 1.0f ? ? ? ? ? ? ? ? // PI observer position gain
#define Ki ?0.001f ? ? ? ? ? ? ? // PI observer integral gain (bias cancellation)
#define dt ?(UPDATE_INTERVAL / 1000000.0f)
??
//对气压计数据进行处理，高度估算。
void getEstimatedAltitude(){
? static uint8_t inited = 0;
? static float AltErrorI = 0.0f;
? static float AccScale ?= 0.0f;
? static uint32_t DeadLine = INIT_DELAY;
? float AltError;
? float InstAcc = 0.0f;
? float Delta;
??
? if (currentTime < DeadLine) return;
? DeadLine = currentTime + UPDATE_INTERVAL;?
? // Soft start
? if (!inited) {
? ? inited = 1;
? ? EstAlt = BaroAlt;
? ? EstVelocity = 0.0f;
? ? AltErrorI = 0.0f;
? ? AccScale = (9.80665f / acc_1G);
? } ?
? // Estimation Error
? AltError = BaroAlt - EstAlt;?
? AltErrorI += AltError;
? // Gravity vector correction and projection to the local Z
? InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
? // Integrators
? Delta = InstAcc * dt + (Kp1 * dt) * AltError;
? EstAlt += ((EstVelocity + Delta * 0.5f) * dt + (Kp2 * dt) * AltError);
? EstVelocity += Delta;
}
附图：
其实IMU程序写的还是有些不足之处的。
1. EstG.A[i]和EstM.A[i]用于ACC和MAG的数据融合和滤波，但其实根本没有起作用；
2.computeIMU其实应该在确定定义了GYRO传感器之后再进行姿态估计。
3. 。。。
总之，我分析的是MultiWii_1_8版本，高版本的代码更加完善与健壮，但大体思路应该相差不大。
