（四）四轴——油门曲线（MWC）

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

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

MWC具体实现代码为：

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

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

具体代码如下：

/************************************************************************************/
/************************************************************************************/
//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代码为：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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一下。不管了，呵呵~

代码如下：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%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;