#elif defined( FLYING_WING )
/***************************** FLYING WING **************************************/
if (!f.ARMED) {
servo[7] = MINCOMMAND; // Kill throttle when disarmed
} else {
servo[7] = constrain(rcCommand[THROTTLE], conf.minthrottle, MAXTHROTTLE);
}
motor[0] = servo[7];
if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing
servo[3] = (SERVODIR(3,1) * rcCommand[PITCH]) + (SERVODIR(3,2) * rcCommand[ROLL]);
servo[4] = (SERVODIR(4,1) * rcCommand[PITCH]) + (SERVODIR(4,2) * rcCommand[ROLL]);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[3] = (SERVODIR(3,1) * axisPID[PITCH]) + (SERVODIR(3,2) * axisPID[ROLL]);
servo[4] = (SERVODIR(4,1) * axisPID[PITCH]) + (SERVODIR(4,2) * axisPID[ROLL]);
}
servo[3] += get_middle(3);
servo[4] += get_middle(4);
#elif defined( AIRPLANE )
/***************************** AIRPLANE **************************************/
// servo[7] is programmed with safty features to avoid motorstarts when ardu reset..
// All other servos go to center at reset.. Half throttle can be dangerus
// Only use servo[7] as motorcontrol if motor is used in the setup */
if (!f.ARMED) {
servo[7] = MINCOMMAND; // Kill throttle when disarmed
} else {
servo[7] = constrain(rcCommand[THROTTLE], conf.minthrottle, MAXTHROTTLE);
}
motor[0] = servo[7];
// Flapperon Controll TODO - optimalisation
int16_t flapperons[2]={0,0};
#if defined(FLAPPERONS) && defined(FLAPPERON_EP)
int8_t flapinv[2] = FLAPPERON_INVERT;
static int16_t F_Endpoint[2] = FLAPPERON_EP;
int16_t flap =MIDRC-constrain(rcData[FLAPPERONS],F_Endpoint[0],F_Endpoint[1]);
static int16_t slowFlaps= flap;
#if defined(FLAPSPEED)
if (slowFlaps < flap ){slowFlaps+=FLAPSPEED;}else if(slowFlaps > flap){slowFlaps-=FLAPSPEED;}
#else
slowFlaps = flap;
#endif
flap = MIDRC-(constrain(MIDRC-slowFlaps,F_Endpoint[0],F_Endpoint[1]));
for(i=0; i<2; i++){flapperons[i] = flap * flapinv[i] ;}
#endif
// Traditional Flaps on SERVO3
#if defined(FLAPS)
// configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control
// use servo min, servo max and servo rate for propper endpoints adjust
int16_t lFlap = get_middle(2);
lFlap = constrain(lFlap, conf.servoConf[2].min, conf.servoConf[2].max);
lFlap = MIDRC - lFlap;
static int16_t slow_LFlaps= lFlap;
#if defined(FLAPSPEED)
if (slow_LFlaps < lFlap ){slow_LFlaps+=FLAPSPEED;} else if(slow_LFlaps > lFlap){slow_LFlaps-=FLAPSPEED;}
#else
slow_LFlaps = lFlap;
#endif
servo[2] = ((int32_t)conf.servoConf[2].rate * slow_LFlaps)/100L;
servo[2] += MIDRC;
#endif
if(f.PASSTHRU_MODE){ // Direct passthru from RX
servo[3] = (SERVODIR(3,1) * rcCommand[PITCH]) + (SERVODIR(3,2) * rcCommand[ROLL]);
servo[4] = (SERVODIR(4,1) * rcCommand[PITCH]) + (SERVODIR(4,2) * rcCommand[ROLL]);
//servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1
//servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2
//servo[5] = rcCommand[YAW]; // Rudder
//servo[6] = rcCommand[PITCH]; // Elevator
}else{
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
servo[3] = (SERVODIR(3,1) * axisPID[PITCH]) + (SERVODIR(3,2) * axisPID[ROLL]);
servo[4] = (SERVODIR(4,1) * axisPID[PITCH]) + (SERVODIR(4,2) * axisPID[ROLL]);
//servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1
//servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2
//servo[5] = axisPID[YAW]; // Rudder
//servo[6] = axisPID[PITCH]; // Elevator
}
for(i=3;i<7;i++) {
servo[i] = ((int32_t)conf.servoConf[i].rate * servo[i])/100L; // servo rates
servo[i] += get_middle(i);
}
