对MWC重要函数的理解

现在四轴炙手可热。由于之前对航模比较感兴趣，因此自然而然对四轴也比较感兴趣。我对四轴了解不多，因此这一系列博客将是个循序渐进的过程。

博客将包括：对四轴原理的理解+算法的研究+对MWC算法的解读与修改。   （MWC程序解读以MultiWii_1_8版本为主，因为版本越高，外设越多，代码越多。我们先只看核心代码。）

关于PPM信号可参考：

http://www.geek-workshop.com/thread-2408-1-1.html

http://diydrones.com/profiles/blogs/705844:BlogPost:38393

http://blog.sina.com.cn/s/blog_658fcbde010155zo.html

注：此图为发射PPM，接收信号高低电平与发射相反。

注：此图为接收PPM。可参照解码。

类似于舵机的控制，每一帧为20ms，再将20ms划分为每2ms一小帧，则一共有10个小帧，也即10个channel。但由于需要加入同步帧，则最多有9个channel。

每一channel有2ms，这2ms由固定的0.5ms再加上可调节的0.5ms~1.5ms构成。

原理清楚了，则可以使用单片机进行解码，为了方便，使用arduino。

[cpp] view plain copy print ?

1. //http://diydrones.com/profiles/blogs/705844:BlogPost:38393  
2. #define channumber 4 //How many channels have your radio?  
3. int value[channumber];  
4.    
5. void setup()  
6. {  
7.         Serial.begin(57600); //Serial Begin  
8.         pinMode(3, INPUT); //Pin 3 as input  
9. }  
10. void loop()  
11. {  
12.         while(pulseIn(3, LOW) < 5000){} //Wait for the beginning of the frame同步帧长度肯定大于5ms，而其他帧的低电平时间肯定小于5ms。  
13.         for(int x=0; x<=channumber-1; x++)//Loop to store all the channel position  
14.         {  
15.                 value[x]=pulseIn(3, LOW);  
16.         }  
17.         for(int x=0; x<=channumber-1; x++)//Loop to print and clear all the channel readings  
18.         {  
19.                 Serial.print(value[x]); //Print the value  
20.                 Serial.print(" ");  
21.                 value[x]=0; //Clear the value afeter is printed  
22.         }  
23.         Serial.println(""); //Start a new line  
24. }  

//http://diydrones.com/profiles/blogs/705844:BlogPost:38393
#define channumber 4 //How many channels have your radio?
int value[channumber];
 
void setup()
{
        Serial.begin(57600); //Serial Begin
        pinMode(3, INPUT); //Pin 3 as input
}
void loop()
{
        while(pulseIn(3, LOW) < 5000){} //Wait for the beginning of the frame同步帧长度肯定大于5ms，而其他帧的低电平时间肯定小于5ms。
        for(int x=0; x<=channumber-1; x++)//Loop to store all the channel position
        {
                value[x]=pulseIn(3, LOW);
        }
        for(int x=0; x<=channumber-1; x++)//Loop to print and clear all the channel readings
        {
                Serial.print(value[x]); //Print the value
                Serial.print(" ");
                value[x]=0; //Clear the value afeter is printed
        }
        Serial.println(""); //Start a new line
}

这里主要使用pulseIn（）函数，因为这个函数可以测得高/低电平持续时间，很是方便。

下面是MWC的解码代码分析。

MWC接收解码在RX文件中进行，MWC支持三种接收方式：

1. （默认）一般PPM信号（也就是接收机对PPM信号帧处理后分到各个接口上去的），一般接收机都是这种。

2. 串口PPM信号，也就是没对信号帧分解的原始信号，例如上面代码解码的就是。

3. SPEKTRUM信号。

假设我们以Arduino pro mini为核心板制作四轴，并且使用SERIAL_SUM_PPM方式接收（例如蓝牙）。黑色为保留的代码，红色为可删除的多余代码。

RX文件代码：

static uint8_t pinRcChannel[8]  = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,CAM1PIN,CAM2PIN};

volatile uint16_t rcPinValue[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]

static int16_t rcData4Values[8][4]; //滤波时候使用，存储最近次channel值。

static int16_t rcDataMean[8] ;

 

// ***PPM SUM SIGNAL***

#ifdef SERIAL_SUM_PPM

static uint8_t rcChannel[8] = {SERIAL_SUM_PPM};//SERIAL_SUM_PPM在config文件中定义，为接收模式定义

#endif

volatile uint16_t rcValue[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]

 

// Configure each rc pin for PCINT

void configureReceiver() {

  #if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM)

    for (uint8_t chan = 0; chan < 8; chan++)

      for (uint8_t a = 0; a < 4; a++)

        rcData4Values[chan][a] = 1500; //we initiate the default value of each channel. If there is no RC receiver connected, we will see those values

    #if defined(PROMINI)

      // PCINT activated only for specific pin inside [D0-D7]  , [D2 D4 D5 D6 D7] for this multicopter

      PORTD   = (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTD (no high impedence PINs)

      PCMSK2 |= (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7); 

      PCICR   = 1<<2; // PCINT activated only for the port dealing with [D0-D7] PINs

    #endif

    #if defined(MEGA)

      // PCINT activated only for specific pin inside [A8-A15]

      DDRK = 0;  // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)

      PORTK   = (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTK

      PCMSK2 |= (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7);

      PCICR   = 1<<2; // PCINT activated only for PORTK dealing with [A8-A15] PINs

    #endif

  #endif

  #if defined(SERIAL_SUM_PPM)

    PPM_PIN_INTERRUPT  //在def文件中定义，为attachInterrupt(0, rxInt, RISING); //PIN 0设置0口中断，产生中断时触发rxInt（）函数。

  #endif

  #if defined (SPEKTRUM)

  

  #endif

}

 

#if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM)

ISR(PCINT2_vect) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a digital pin [D2-D7]

  uint8_t mask;

  uint8_t pin;

  uint16_t cTime,dTime;

  static uint16_t edgeTime[8];

  static uint8_t PCintLast;

 

  #if defined(PROMINI)

    pin = PIND;             // PIND indicates the state of each PIN for the arduino port dealing with [D0-D7] digital pins (8 bits variable)

  #endif

  #if defined(MEGA)

    pin = PINK;             // PINK indicates the state of each PIN for the arduino port dealing with [A8-A15] digital pins (8 bits variable)

  #endif

  mask = pin ^ PCintLast;   // doing a ^ between the current interruption and the last one indicates wich pin changed

  sei();                    // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely

  PCintLast = pin;          // we memorize the current state of all PINs [D0-D7]

 

  cTime = micros();         // micros() return a uint32_t, but it is not usefull to keep the whole bits => we keep only 16 bits

  

  // mask is pins [D0-D7] that have changed // the principle is the same on the MEGA for PORTK and [A8-A15] PINs

  // chan = pin sequence of the port. chan begins at D2 and ends at D7

  if (mask & 1<<2)           //indicates the bit 2 of the arduino port [D0-D7], that is to say digital pin 2, if 1 => this pin has just changed

    if (!(pin & 1<<2)) {     //indicates if the bit 2 of the arduino port [D0-D7] is not at a high state (so that we match here only descending PPM pulse)

      dTime = cTime-edgeTime[2]; if (900<dTime && dTime<2200) rcPinValue[2] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin

    } else edgeTime[2] = cTime;    // if the bit 2 of the arduino port [D0-D7] is at a high state (ascending PPM pulse), we memorize the time

  if (mask & 1<<4)   //same principle for other channels   // avoiding a for() is more than twice faster, and it's important to minimize execution time in ISR

    if (!(pin & 1<<4)) {

      dTime = cTime-edgeTime[4]; if (900<dTime && dTime<2200) rcPinValue[4] = dTime;

    } else edgeTime[4] = cTime;

  if (mask & 1<<5)

    if (!(pin & 1<<5)) {

      dTime = cTime-edgeTime[5]; if (900<dTime && dTime<2200) rcPinValue[5] = dTime;

    } else edgeTime[5] = cTime;

  if (mask & 1<<6)

    if (!(pin & 1<<6)) {

      dTime = cTime-edgeTime[6]; if (900<dTime && dTime<2200) rcPinValue[6] = dTime;

    } else edgeTime[6] = cTime;

  if (mask & 1<<7)

    if (!(pin & 1<<7)) {

      dTime = cTime-edgeTime[7]; if (900<dTime && dTime<2200) rcPinValue[7] = dTime;

    } else edgeTime[7] = cTime;

  #if defined(MEGA)

    if (mask & 1<<0)    

      if (!(pin & 1<<0)) {

        dTime = cTime-edgeTime[0]; if (900<dTime && dTime<2200) rcPinValue[0] = dTime; 

      } else edgeTime[0] = cTime; 

    if (mask & 1<<1)      

      if (!(pin & 1<<1)) {

        dTime = cTime-edgeTime[1]; if (900<dTime && dTime<2200) rcPinValue[1] = dTime; 

      } else edgeTime[1] = cTime;

    if (mask & 1<<3)

      if (!(pin & 1<<3)) {

        dTime = cTime-edgeTime[3]; if (900<dTime && dTime<2200) rcPinValue[3] = dTime;

      } else edgeTime[3] = cTime;

  #endif

  #if defined(FAILSAFE)

    if (mask & 1<<THROTTLEPIN) {    // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter  - added by MIS

      if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; }

  #endif

}

#endif

 

#if defined(SERIAL_SUM_PPM)

//接收，将每个channel数据存到对应的rcValue[channel]中。

void rxInt() {

  uint16_t now,diff;

  static uint16_t last = 0;

  static uint8_t chan = 0;

 

  now = micros();

  diff = now - last;  //获得每个channel时间

  last = now;

  if(diff>3000) chan = 0; //接收到同步帧

  else {

    if(900<diff && diff<2200 && chan<8 ) {   //Only if the signal is between these values it is valid, otherwise the failsafe counter should move up 1ms~2ms

      rcValue[chan] = diff;

      #if defined(FAILSAFE)//掉电保护

        if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0;   // clear FailSafe counter - added by MIS  //incompatible to quadroppm

      #endif

 

    }

    chan++;

  }

}

#endif

 

#if defined(SPEKTRUM)

 

 

#endif

 

//为了准确，在关闭中断的时候读取接收的chan信道数据rcValue[chan]

uint16_t readRawRC(uint8_t chan) {

  uint16_t data;

  uint8_t oldSREG;

  oldSREG = SREG;

  cli(); // Let's disable interrupts

  #ifndef SERIAL_SUM_PPM

    data = rcPinValue[pinRcChannel[chan]]; // Let's copy the data Atomically

  #else

    data = rcValue[rcChannel[chan]]; 

  #endif

  SREG = oldSREG;

  sei();// Let's enable the interrupts

  #if defined(PROMINI) && !defined(SERIAL_SUM_PPM)

  if (chan>4) return 1500;

  #endif

  return data; // We return the value correctly copied when the IRQ's where disabled

}

  

//对数据进行滤波计算操作

void computeRC() {

  static uint8_t rc4ValuesIndex = 0;

  uint8_t chan,a;

 

  rc4ValuesIndex++;

  for (chan = 0; chan < 8; chan++) {

    rcData4Values[chan][rc4ValuesIndex%4] = readRawRC(chan);//rcData4Values[chan][]保存最近四次chan信道的数据

    rcDataMean[chan] = 0;

    for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];

    rcDataMean[chan]= (rcDataMean[chan]+2)/4;//求取最近4次chan信道值的平均值

    if ( rcDataMean[chan] < rcData[chan] -3)  rcData[chan] = rcDataMean[chan]+2;

    if ( rcDataMean[chan] > rcData[chan] +3)  rcData[chan] = rcDataMean[chan]-2;//并没有直接将rcDataMean[chan]赋值给rcData[chan]，而是慢慢修正rcData[chan]的值，稳定性更高。rcData[8]在主文件里面定义。

  }

}

 

流程图就不画了，描述下吧：

 rxInt() 接收PPM帧信号，将每个信道的数据存储到rcValue[chan]里面，之后可以用readRawRC（chan）函数在关闭中断时候准确读出每个信道的值，再用computeRC（）函数对每个信道取最近4次值，之后计算平均值，并依此修正 rcData[chan]。可见rcData[chan]为我们最终要修改的值，也是后面的控制算法使用的值。

 

可参照此图理解上面的中断解码。

如有错误请指正，我们共同进步。

对MWC重要函数的理解
目录
一、原文件的理解
二、我对main loop()主程序的分析
三、conputeIMU函数以及它调用的估计姿态函数getEstimatedAttitude()函数的分析
四、旋转矩阵求坐标解析 五、计算俯仰角度的依据文件 
              

 
2
一、原文件的理解
void computeIMU () {   uint8_t axis;
  static int16_t gyroADCprevious[3] = {0,0,0};  

  int16_t gyroADCp[3];  

  int16_t gyroADCinter[3];
  static uint32_t timeInterleave = 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 defined(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()-timeInterleave)<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, 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])/4;      

     gyroADCprevious[axis] = gyroADC[axis];     }  

      #else
    #if ACC//……………………………………………………….①       ACC_getADC();//获得加计的ADC值
      getEstimatedAttitude();//获取估计姿态    

      #endif    

    #if GYRO
      Gyro_getADC();    

      #endif
    for (axis = 0; axis < 3; axis++)
      gyroADCp[axis] =  gyroADC[axis];    

     timeInterleave=micros();
     annexCode();//通过串口向电脑或GUI发送MWC的实时数据。    

     if ((micros()-timeInterleave)>650) {
       annex650_overrun_count++;//运行时间超时 并进行计数

    }

else {
       while((micros()-timeInterleave)<650)  //empirical, interleaving delay between 2 consecutive reads  //当运行时间不足650ms时 依据验证 进行2次连续的读内部延时      }
    #if GYRO
      Gyro_getADC();   

      #endif
     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])/3;      

    gyroADCprevious[axis] = gyroADCinter[axis]/2;      

    if (!ACC) accADC[axis]=0;     }  

   #endif
  #if defined(GYRO_SMOOTHING)
    static int16_t gyroSmooth[3] = {0,0,0};//静态变量设置 第一次运行时进行初始化，以后保留前次值再更新。    

    for (axis = 0; axis < 3; axis++) {    

      gyroData[axis] = (int16_t) ( ( (int32_t)((int32_t)gyroSmooth[axis] * (conf.Smoothing[axis]-1) )+gyroData[axis]+1 ) / conf.Smoothing[axis]);//陀螺仪数据平滑滤波       gyroSmooth[axis] = gyroData[axis];     }
  #elif defined(TRI)//假如定义为三脚机   

    static int16_t gyroYawSmooth = 0;
    gyroData[YAW] = (gyroYawSmooth*2+gyroData[YAW])/3;    

   gyroYawSmooth = gyroData[YAW];   #endif } 
// **************************************************
// Simplified IMU based on "Complementary Filter"//基于互补滤波简化的IMU计算 // Inspired by 创新来自于http://starlino.com/imu_guide.html //
// adapted by 改编来自于ziss_dm : http://www.multiwii.com/forum/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() //antan2的近似算法 // 4) Optimization tricks优化:http://www.hackersdelight.org/ //
// Currently Magnetometer uses separate CF which is used only // for heading approximation.//磁力计 用于近似定向 //

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)<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,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])<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版本，高版本的代码更加完善与健壮，但大体思路应该相差不大。

Sensors 传感器操作（MWC）

MWC的传感器函数在Sensors文件中定义，该文件中主要实现对各种传感器的操作。

1. 主要支持：ACC（加速度计）、Gyro（陀螺仪）、Mag（电磁传感器）、Baro（气压传感器）

2.  gyroADC[3],

accADC[3],

magADC[3]，

BaroAlt //分别存放传感器读取的数据。

MWC的Sensors文件对传感器的操作可以简化为以下描述：

//ACC相关操作

void Acc_getADC () {  //获取accADC[3]
}
void ACC_init() {
  acc_1G = 256;   //这是 加速度的精度，1g时候对应的数值。不同传感器数据不同。
}

//Gyro相关操作
void Gyro_getADC () {
}
void Gyro_init() {
}

//Mag相关操作
void Mag_getADC() {
}
void Mag_init() {
}

//Baro相关操作
void Baro_update() {
}
void Baro_init() {
}


void initSensors() {
  delay(100);
  if (ACC) ACC_init();
  if (GYRO) Gyro_init();
  if (MAG) Mag_init();
  if (BARO) Baro_init();
}

附图形说明：

四轴——油门曲线（MWC）

接收的PPM信号为1000~2000ms，为了便于PID控制，需要将信号进行处理。

将ROLL/PITCH/YAW   变为 -500~500范围，将THROTTLE变为1000~2000范围。

MWC具体实现代码为：

1. 先定义lookup函数，用于映射，在EEPROM中；

2. 根据接收的PPM信号进行映射，处理到相应的数据范围。

具体代码如下：

[cpp] view plain copy print ?

1. /************************************************************************************/  
2. /************************************************************************************/  
3. //set lookup  
4.   
5. rcRate8 = 90;   
6. rcExpo8 = 65;  
7. for(i=0;i<5;i++) {  
8.     lookupPitchRollRC[i] = (1526+rcExpo8*(i*i-15))*i*(int32_t)rcRate8/1192;  
9. }  
10.   
11. //Motor maxthrottle  
12. /* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */  
13.     #define MAXTHROTTLE 1850  
14. //Motor minthrottle    
15.     /* Set the minimum throttle command sent to the ESC (Electronic Speed Controller) 
16.        This is the minimum value that allow motors to run at a idle speed  */  
17.     //#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A  
18.     //#define MINTHROTTLE 1120 // for Super Simple ESCs 10A  
19.     //#define MINTHROTTLE 1064 // special ESC (simonk)  
20.     //#define MINTHROTTLE 1050 // for brushed ESCs like ladybird  
21.     #define MINTHROTTLE 1150 // (*) (**)  
22. thrMid8 = 50;  
23. thrExpo8 = 0;  
24. for(i=0;i<11;i++) {  
25.     int16_t tmp = 10*i-thrMid8;  
26.     uint8_t y = 1;  
27.     if (tmp>0) y = 100-thrMid8;  
28.     if (tmp<0) y = thrMid8;  
29.     lookupThrottleRC[i] = 10*thrMid8 + tmp*( 100-thrExpo8+(int32_t)thrExpo8*(tmp*tmp)/(y*y) )/10; // [0;1000]  
30.     lookupThrottleRC[i] = MINTHROTTLE + (int32_t)(MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC[i]/1000;  // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]  
31. }  
32.   
33. /************************************************************************************/  
34. /************************************************************************************/  
35. #define MIDRC 1500  
36.   uint16_t tmp,tmp2;  
37.   uint8_t axis,prop1,prop2;  
38.   dynThrPID = 0;  
39.   
40.   // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value  
41.   prop2 = 128; // prop2 was 100, is 128 now  
42.   if (rcData[THROTTLE]>1500) { // breakpoint is fix: 1500  
43.     if (rcData[THROTTLE]<2000) {  
44.       prop2 -=  ((uint16_t)dynThrPID*(rcData[THROTTLE]-1500)>>9); //  /512 instead of /500  
45.     } else {  
46.       prop2 -=  dynThrPID;  
47.     }  
48.   }  
49.   
50.   for(axis=0;axis<3;axis++) {  
51.     tmp = min(abs(rcData[axis]-MIDRC),500);  
52.     if(axis!=2) { //ROLL & PITCH  
53.       tmp2 = tmp>>7; // 500/128 = 3.9  => range [0;3]  
54.       rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7);  
55.       prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500  
56.       prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128   prop2: max is 128   result prop1: max is 128  
57.       dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now  
58.       dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now  
59.     } else {      // YAW  
60.       rcCommand[axis] = tmp;  
61.     }  
62.     if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];      //-500 ~ 500  
63.   }  
64.   tmp = constrain(rcData[THROTTLE],MINCHECK,2000);  
65.   tmp = (uint32_t)(tmp-MINCHECK)*2559/(2000-MINCHECK); // [MINCHECK;2000] -> [0;2559]  
66.   tmp2 = tmp/256; // range [0;9]  
67.   rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]  

/************************************************************************************/
/************************************************************************************/
//set lookup

rcRate8 = 90; 
rcExpo8 = 65;
for(i=0;i<5;i++) {
    lookupPitchRollRC[i] = (1526+rcExpo8*(i*i-15))*i*(int32_t)rcRate8/1192;
}

//Motor maxthrottle
/* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */
    #define MAXTHROTTLE 1850
//Motor minthrottle  
    /* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)
       This is the minimum value that allow motors to run at a idle speed  */
    //#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
    //#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
    //#define MINTHROTTLE 1064 // special ESC (simonk)
    //#define MINTHROTTLE 1050 // for brushed ESCs like ladybird
    #define MINTHROTTLE 1150 // (*) (**)
thrMid8 = 50;
thrExpo8 = 0;
for(i=0;i<11;i++) {
    int16_t tmp = 10*i-thrMid8;
    uint8_t y = 1;
    if (tmp>0) y = 100-thrMid8;
    if (tmp<0) y = thrMid8;
    lookupThrottleRC[i] = 10*thrMid8 + tmp*( 100-thrExpo8+(int32_t)thrExpo8*(tmp*tmp)/(y*y) )/10; // [0;1000]
    lookupThrottleRC[i] = MINTHROTTLE + (int32_t)(MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC[i]/1000;  // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}

/************************************************************************************/
/************************************************************************************/
#define MIDRC 1500
  uint16_t tmp,tmp2;
  uint8_t axis,prop1,prop2;
  dynThrPID = 0;

  // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
  prop2 = 128; // prop2 was 100, is 128 now
  if (rcData[THROTTLE]>1500) { // breakpoint is fix: 1500
    if (rcData[THROTTLE]<2000) {
      prop2 -=  ((uint16_t)dynThrPID*(rcData[THROTTLE]-1500)>>9); //  /512 instead of /500
    } else {
      prop2 -=  dynThrPID;
    }
  }

  for(axis=0;axis<3;axis++) {
    tmp = min(abs(rcData[axis]-MIDRC),500);
    if(axis!=2) { //ROLL & PITCH
      tmp2 = tmp>>7; // 500/128 = 3.9  => range [0;3]
      rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7);
      prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500
      prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128   prop2: max is 128   result prop1: max is 128
      dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now
      dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now
    } else {      // YAW
      rcCommand[axis] = tmp;
    }
    if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];      //-500 ~ 500
  }
  tmp = constrain(rcData[THROTTLE],MINCHECK,2000);
  tmp = (uint32_t)(tmp-MINCHECK)*2559/(2000-MINCHECK); // [MINCHECK;2000] -> [0;2559]
  tmp2 = tmp/256; // range [0;9]
  rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]

为了更加直观的看出具体的曲线，使用Matlab对其绘制：

（1）lookup函数曲线：

红色为ROLL和PITCH的lookup曲线；

蓝色为THROTTLE的lookup曲线。

matlab代码为：

[plain] view plain copy print ?

1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%set lookup%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
2. %Roll/Pitch  
3. rcRate = 90;   
4. rcExpo = 65;  
5. lookupPitchRollRC=[0 0 0 0 0 0];  
6. for i=1:1:5     %1~5  
7.     lookupPitchRollRC(i) = (1526+rcExpo*((i-1)*(i-1)-15))*(i-1)*rcRate/1192;  
8. end;  
9. plot(lookupPitchRollRC,'r');  
10. hold on;  
11. stem(lookupPitchRollRC,'r');  
12.   
13. %Throttle  
14. MAXTHROTTLE = 1850;  
15. MINTHROTTLE = 1150;  
16. thrMid = 50;  
17. thrExpo = 0;  
18. lookupThrottleRC=[0 0 0 0 0 0 0 0 0 0 0];  
19. for i=1:1:11  %1~11  
20.     tmp = 10*(i-1)-thrMid;  
21.     y = 1;  
22.     if (tmp>0)   
23.         y = 100-thrMid;  
24.     end  
25.     if (tmp<0)   
26.         y = thrMid;  
27.     end  
28.     lookupThrottleRC(i) = 10*thrMid + tmp*( 100-thrExpo+thrExpo*(tmp*tmp)/(y*y) )/10; % [0;1000]  
29.     lookupThrottleRC(i) = MINTHROTTLE + (MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC(i)/1000;  % [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]  
30. end  
31. plot(lookupThrottleRC,'b');  
32. stem(lookupThrottleRC,'b');  
33. hold off;  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%set lookup%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Roll/Pitch
rcRate = 90; 
rcExpo = 65;
lookupPitchRollRC=[0 0 0 0 0 0];
for i=1:1:5     %1~5
    lookupPitchRollRC(i) = (1526+rcExpo*((i-1)*(i-1)-15))*(i-1)*rcRate/1192;
end;
plot(lookupPitchRollRC,'r');
hold on;
stem(lookupPitchRollRC,'r');

%Throttle
MAXTHROTTLE = 1850;
MINTHROTTLE = 1150;
thrMid = 50;
thrExpo = 0;
lookupThrottleRC=[0 0 0 0 0 0 0 0 0 0 0];
for i=1:1:11  %1~11
    tmp = 10*(i-1)-thrMid;
    y = 1;
    if (tmp>0) 
        y = 100-thrMid;
    end
    if (tmp<0) 
        y = thrMid;
    end
    lookupThrottleRC(i) = 10*thrMid + tmp*( 100-thrExpo+thrExpo*(tmp*tmp)/(y*y) )/10; % [0;1000]
    lookupThrottleRC(i) = MINTHROTTLE + (MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC(i)/1000;  % [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
end
plot(lookupThrottleRC,'b');
stem(lookupThrottleRC,'b');
hold off;

（2）使用lookup函数进行数据映射处理：

红色为Yaw的rcCommand曲线；蓝色为Roll和Pitch的rcCommand曲线；绿色的为THROTTLE曲线。

somehow... why the blue beyond it's ...   或许roll和pitch需要constrain一下。不管了，呵呵~

代码如下：

[plain] view plain copy print ?

1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%rcCommand%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
2. MIDRC = 1500;  
3. dynThrPID = 0;  
4. THROTTLE = 4;   
5. MINCHECK = 1150;  
6.   
7. %ROLL and PITCH   //rcData -> rcCommand  
8. rcData = 1000:1:2000;  
9. tmp = min(abs(rcData-MIDRC),500);  
10. tmp_t =floor(tmp/128); % 500/128 = 3.9  => range [0;3]  
11. rcCommand_RO_PI = lookupPitchRollRC(tmp_t+1) + ((tmp-(tmp_t/128)).*(lookupPitchRollRC(tmp_t+2)-lookupPitchRollRC(tmp_t+1))/128);  %0~500  
12. for j=1:length(rcData)  
13.     if (rcData(j)<MIDRC)   
14.         rcCommand_RO_PI(j) = -rcCommand_RO_PI(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'  
15.     end  
16. end  
17. plot(rcData,rcCommand_RO_PI,'b');  
18. hold on;  
19.   
20. %YAW   //rcData -> rcCommand  
21. rcCommand_YA = tmp;  
22. for j=1:length(rcData)  
23.     if (rcData(j)<MIDRC)   
24.         rcCommand_YA(j) = -rcCommand_YA(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'  
25.     end  
26. end  
27. plot(rcData,rcCommand_YA,'r');  
28.   
29. %THROTTLE //rcData -> rcCommand  
30. THROTTLE_tmp = max(rcData,MINCHECK);   %[MINCHECK,2000]  
31. THROTTLE_tmp =(THROTTLE_tmp-MINCHECK)*2559/(2000-MINCHECK); % [MINCHECK;2000] -> [0;2559]  
32. THROTTLE_tmp_t = floor(THROTTLE_tmp/256); % range [0;9]  
33. rcCommand_TH = lookupThrottleRC(THROTTLE_tmp_t+1) + (THROTTLE_tmp-THROTTLE_tmp_t*256).*(lookupThrottleRC(THROTTLE_tmp_t+2)-lookupThrottleRC(THROTTLE_tmp_t+1))/256; % [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]  
34. plot(rcData,rcCommand_TH,'g');  
35. hold off;  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%rcCommand%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MIDRC = 1500;
dynThrPID = 0;
THROTTLE = 4; 
MINCHECK = 1150;

%ROLL and PITCH   //rcData -> rcCommand
rcData = 1000:1:2000;
tmp = min(abs(rcData-MIDRC),500);
tmp_t =floor(tmp/128); % 500/128 = 3.9  => range [0;3]
rcCommand_RO_PI = lookupPitchRollRC(tmp_t+1) + ((tmp-(tmp_t/128)).*(lookupPitchRollRC(tmp_t+2)-lookupPitchRollRC(tmp_t+1))/128);  %0~500
for j=1:length(rcData)
    if (rcData(j)<MIDRC) 
        rcCommand_RO_PI(j) = -rcCommand_RO_PI(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'
    end
end
plot(rcData,rcCommand_RO_PI,'b');
hold on;

%YAW   //rcData -> rcCommand
rcCommand_YA = tmp;
for j=1:length(rcData)
    if (rcData(j)<MIDRC) 
        rcCommand_YA(j) = -rcCommand_YA(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'
    end
end
plot(rcData,rcCommand_YA,'r');

%THROTTLE //rcData -> rcCommand
THROTTLE_tmp = max(rcData,MINCHECK);   %[MINCHECK,2000]
THROTTLE_tmp =(THROTTLE_tmp-MINCHECK)*2559/(2000-MINCHECK); % [MINCHECK;2000] -> [0;2559]
THROTTLE_tmp_t = floor(THROTTLE_tmp/256); % range [0;9]
rcCommand_TH = lookupThrottleRC(THROTTLE_tmp_t+1) + (THROTTLE_tmp-THROTTLE_tmp_t*256).*(lookupThrottleRC(THROTTLE_tmp_t+2)-lookupThrottleRC(THROTTLE_tmp_t+1))/256; % [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
plot(rcData,rcCommand_TH,'g');
hold off;

顶

/************************************************************************************/
/************************************************************************************/
//set lookup

rcRate8 = 90; 
rcExpo8 = 65;
for(i=0;i<5;i++) {
    lookupPitchRollRC[i] = (1526+rcExpo8*(i*i-15))*i*(int32_t)rcRate8/1192;
}

//Motor maxthrottle
/* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */
    #define MAXTHROTTLE 1850
//Motor minthrottle  
    /* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)
       This is the minimum value that allow motors to run at a idle speed  */
    //#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
    //#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
    //#define MINTHROTTLE 1064 // special ESC (simonk)
    //#define MINTHROTTLE 1050 // for brushed ESCs like ladybird
    #define MINTHROTTLE 1150 // (*) (**)
thrMid8 = 50;
thrExpo8 = 0;
for(i=0;i<11;i++) {
    int16_t tmp = 10*i-thrMid8;
    uint8_t y = 1;
    if (tmp>0) y = 100-thrMid8;
    if (tmp<0) y = thrMid8;
    lookupThrottleRC[i] = 10*thrMid8 + tmp*( 100-thrExpo8+(int32_t)thrExpo8*(tmp*tmp)/(y*y) )/10; // [0;1000]
    lookupThrottleRC[i] = MINTHROTTLE + (int32_t)(MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC[i]/1000;  // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
}

/************************************************************************************/
/************************************************************************************/
#define MIDRC 1500
  uint16_t tmp,tmp2;
  uint8_t axis,prop1,prop2;
  dynThrPID = 0;

  // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
  prop2 = 128; // prop2 was 100, is 128 now
  if (rcData[THROTTLE]>1500) { // breakpoint is fix: 1500
    if (rcData[THROTTLE]<2000) {
      prop2 -=  ((uint16_t)dynThrPID*(rcData[THROTTLE]-1500)>>9); //  /512 instead of /500
    } else {
      prop2 -=  dynThrPID;
    }
  }

  for(axis=0;axis<3;axis++) {
    tmp = min(abs(rcData[axis]-MIDRC),500);
    if(axis!=2) { //ROLL & PITCH
      tmp2 = tmp>>7; // 500/128 = 3.9  => range [0;3]
      rcCommand[axis] = lookupPitchRollRC[tmp2] + ((tmp-(tmp2<<7)) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2])>>7);
      prop1 = 128-((uint16_t)conf.rollPitchRate*tmp>>9); // prop1 was 100, is 128 now -- and /512 instead of /500
      prop1 = (uint16_t)prop1*prop2>>7; // prop1: max is 128   prop2: max is 128   result prop1: max is 128
      dynP8[axis] = (uint16_t)conf.pid[axis].P8*prop1>>7; // was /100, is /128 now
      dynD8[axis] = (uint16_t)conf.pid[axis].D8*prop1>>7; // was /100, is /128 now
    } else {      // YAW
      rcCommand[axis] = tmp;
    }
    if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];      //-500 ~ 500
  }
  tmp = constrain(rcData[THROTTLE],MINCHECK,2000);
  tmp = (uint32_t)(tmp-MINCHECK)*2559/(2000-MINCHECK); // [MINCHECK;2000] -> [0;2559]
  tmp2 = tmp/256; // range [0;9]
  rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*256) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 256; // [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%set lookup%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Roll/Pitch
rcRate = 90; 
rcExpo = 65;
lookupPitchRollRC=[0 0 0 0 0 0];
for i=1:1:5     %1~5
    lookupPitchRollRC(i) = (1526+rcExpo*((i-1)*(i-1)-15))*(i-1)*rcRate/1192;
end;
plot(lookupPitchRollRC,'r');
hold on;
stem(lookupPitchRollRC,'r');

%Throttle
MAXTHROTTLE = 1850;
MINTHROTTLE = 1150;
thrMid = 50;
thrExpo = 0;
lookupThrottleRC=[0 0 0 0 0 0 0 0 0 0 0];
for i=1:1:11  %1~11
    tmp = 10*(i-1)-thrMid;
    y = 1;
    if (tmp>0) 
        y = 100-thrMid;
    end
    if (tmp<0) 
        y = thrMid;
    end
    lookupThrottleRC(i) = 10*thrMid + tmp*( 100-thrExpo+thrExpo*(tmp*tmp)/(y*y) )/10; % [0;1000]
    lookupThrottleRC(i) = MINTHROTTLE + (MAXTHROTTLE-MINTHROTTLE)* lookupThrottleRC(i)/1000;  % [0;1000] -> [MINTHROTTLE;MAXTHROTTLE]
end
plot(lookupThrottleRC,'b');
stem(lookupThrottleRC,'b');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%rcCommand%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MIDRC = 1500;
dynThrPID = 0;
THROTTLE = 4; 
MINCHECK = 1150;

%ROLL and PITCH   //rcData -> rcCommand
rcData = 1000:1:2000;
tmp = min(abs(rcData-MIDRC),500);
tmp_t =floor(tmp/128); % 500/128 = 3.9  => range [0;3]
rcCommand_RO_PI = lookupPitchRollRC(tmp_t+1) + ((tmp-(tmp_t/128)).*(lookupPitchRollRC(tmp_t+2)-lookupPitchRollRC(tmp_t+1))/128);  %0~500
for j=1:length(rcData)
    if (rcData(j)<MIDRC) 
        rcCommand_RO_PI(j) = -rcCommand_RO_PI(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'
    end
end
plot(rcData,rcCommand_RO_PI,'b');
hold on;

%YAW   //rcData -> rcCommand
rcCommand_YA = tmp;
for j=1:length(rcData)
    if (rcData(j)<MIDRC) 
        rcCommand_YA(j) = -rcCommand_YA(j);      %-500 ~ 500    //when rcData<MIDRC  is '-'
    end
end
plot(rcData,rcCommand_YA,'r');

%THROTTLE //rcData -> rcCommand
THROTTLE_tmp = max(rcData,MINCHECK);   %[MINCHECK,2000]
THROTTLE_tmp =(THROTTLE_tmp-MINCHECK)*2559/(2000-MINCHECK); % [MINCHECK;2000] -> [0;2559]
THROTTLE_tmp_t = floor(THROTTLE_tmp/256); % range [0;9]
rcCommand_TH = lookupThrottleRC(THROTTLE_tmp_t+1) + (THROTTLE_tmp-THROTTLE_tmp_t*256).*(lookupThrottleRC(THROTTLE_tmp_t+2)-lookupThrottleRC(THROTTLE_tmp_t+1))/256; % [0;2559] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
plot(rcData,rcCommand_TH,'g');
hold off;