接收的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;