void computeRC() {
static uint16_t rcData4Values[RC_CHANS][4], rcDataMean[RC_CHANS];
static uint8_t rc4ValuesIndex = 0;
uint8_t axis;
uint8_t chan,a;
#if defined(TRACE)
Serial.println(">computeRC");
#endif
rc4ValuesIndex++;
if (rc4ValuesIndex == 4) rc4ValuesIndex = 0;
for (chan = 0; chan < RC_CHANS; chan++) { // read data from all channels
rcData4Values[chan][rc4ValuesIndex] = readRawRC(chan);
#if defined(TRACE2)
Serial.print("rcData4Values[");Serial.print((int)chan);Serial.print("][");
Serial.print((int)rc4ValuesIndex);Serial.print("]:");Serial.println(rcData4Values[chan][rc4ValuesIndex]);
#endif
rcDataMean[chan] = 0;
for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a]; // make average on 4 values
rcDataMean[chan]= (rcDataMean[chan]+2)>>2;
if ( rcDataMean[chan] < (uint16_t)rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2; // filter in current value outside +/- 3 previous value
if ( rcDataMean[chan] > (uint16_t)rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
#if defined(TRACE)
Serial.print("rcData[");Serial.print((int)chan);Serial.print("]:");Serial.println(rcData[chan]);
#endif
} // end read data from all channels
//ROLL & PITCH & YAW
for(axis=0;axis<3;axis++) {
rcCommand[axis] = min(abs(rcData[axis]-MIDRC),500); // interval [#1000;#2000]
if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis]; // translated to interval [-500; +500]
#if defined(TRACE)
Serial.print("rcCommand[");Serial.print((int)axis);Serial.print("]:");Serial.println(rcCommand[axis]);
#endif
} // end for ROLL & PITCH & YAW
// THROTTLE
rcCommand[THROTTLE] = constrain(rcData[THROTTLE],MINTHROTTLE,MAXTHROTTLE); // interval [#1000;#2000] restricted to interval [MINTHROTTLE; MAXTHROTTLE]
// usually MINTHROTTLE = 1150, MAXTHROTTLE = 1850
#if defined(TRACE)
Serial.print("rcCommand[");Serial.print((int)THROTTLE);Serial.print("]:");Serial.println(rcCommand[THROTTLE]);
#endif
}
void computeRC() {
static uint16_t rcData4Values[RC_CHANS][4], rcDataMean[RC_CHANS];
static uint8_t rc4ValuesIndex = 0;
uint16_t tmp,tmp2;
uint8_t axis,prop1,prop2;
uint8_t chan,a;
#if defined(TRACE)
Serial.println(">computeRC");
#endif
rc4ValuesIndex++;
if (rc4ValuesIndex == 4) rc4ValuesIndex = 0;
for (chan = 0; chan < RC_CHANS; chan++) { // read data from all channels
rcData4Values[chan][rc4ValuesIndex] = readRawRC(chan);
#if defined(TRACE2)
Serial.print("rcData4Values[");Serial.print((int)chan);Serial.print("][");
Serial.print((int)rc4ValuesIndex);Serial.print("]:");Serial.println(rcData4Values[chan][rc4ValuesIndex]);
#endif
rcDataMean[chan] = 0;
for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a]; // make average on 4 values
rcDataMean[chan]= (rcDataMean[chan]+2)>>2;
if ( rcDataMean[chan] < (uint16_t)rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2; // filter in current value outside +/- 3 previous value
if ( rcDataMean[chan] > (uint16_t)rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
#if defined(TRACE)
Serial.print("rcData[");Serial.print((int)chan);Serial.print("]:");Serial.println(rcData[chan]);
#endif
} // end read data from all channels
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
prop2 = 128;
if (rcData[THROTTLE]>1500) { // breakpoint is fix: 1500
if (rcData[THROTTLE]<2000) {
prop2 -= ((uint16_t)conf.dynThrPID*(rcData[THROTTLE]-1500)>>9);
} else {
