（三）四轴——IMU传感器数据处理（MWC）

接上篇。

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,0};
  int16_t gyroADCp[3];
  int16_t gyroADCinter[3];
  static int16_t lastAccADC[3] = {0,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)     timeInterleave=micros();
    WMP_getRawADC();
    getEstimatedAttitude(); // computation time must last less than one interleaving delay
    #if BARO
      getEstimatedAltitude();
    #endif 
    while((micros()-timeInterleave)     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,0,300};
  static t_fp_vector EstM = {0,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])     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版本，高版本的代码更加完善与健壮，但大体思路应该相差不大。