现在四轴炙手可热。由于之前对航模比较感兴趣,因此自然而然对四轴也比较感兴趣。我对四轴了解不多,因此这一系列博客将是个循序渐进的过程。
博客将包括:对四轴原理的理解+算法的研究+对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。
//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
}
下面是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
} 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
} else edgeTime[4] = cTime;
if (mask & 1<<5)
if (!(pin & 1<<5)) {
dTime = cTime-edgeTime[5]; if (900
} else edgeTime[5] = cTime;
if (mask & 1<<6)
if (!(pin & 1<<6)) {
dTime = cTime-edgeTime[6]; if (900
} else edgeTime[6] = cTime;
if (mask & 1<<7)
if (!(pin & 1<<7)) {
dTime = cTime-edgeTime[7]; if (900
} else edgeTime[7] = cTime;
#if defined(MEGA)
if (mask & 1<<0)
if (!(pin & 1<<0)) {
dTime = cTime-edgeTime[0]; if (900
} else edgeTime[0] = cTime;
if (mask & 1<<1)
if (!(pin & 1<<1)) {
dTime = cTime-edgeTime[1]; if (900
} else edgeTime[1] = cTime;
if (mask & 1<<3)
if (!(pin & 1<<3)) {
dTime = cTime-edgeTime[3]; if (900
} else edgeTime[3] = cTime;
#endif
#if defined(FAILSAFE)
if (mask & 1<
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 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]为我们最终要修改的值,也是后面的控制算法使用的值。 可参照此图理解上面的中断解码。 如有错误请指正,我们共同进步。