Flaperon flight mode (#307)

* basic flaperon functionality
* flaperon configuration
* FLAPERON can be activated only on MIXER_AIRPLANE and MIXER_CUSTOM_AIRPLANE
* state FLAPERON_AVAILABLE added

src/main/config/config.c

@@ -604,6 +604,10 @@ static void resetConf(void)
 
     // gimbal
     currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
+
+    currentProfile->flaperon_throw_offset = FLAPERON_THROW_DEFAULT;
+    currentProfile->flaperon_throw_inverted = 0;
+
 #endif
 
     // custom mixer. clear by defaults.

src/main/config/config_profile.h

@@ -40,5 +40,8 @@ typedef struct profile_s {
     servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
     // gimbal-related configuration
     gimbalConfig_t gimbalConfig;
+
+    uint16_t flaperon_throw_offset;
+    uint8_t flaperon_throw_inverted;
 #endif
 } profile_t;

src/main/config/runtime_config.h

@@ -45,6 +45,7 @@ typedef enum {
     UNUSED_MODE2    = (1 << 11), // old G-Tune
     NAV_WP_MODE     = (1 << 12),
     HEADING_LOCK    = (1 << 13),
+    FLAPERON        = (1 << 14),
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;
@@ -54,12 +55,14 @@ extern uint16_t flightModeFlags;
 #define FLIGHT_MODE(mask) (flightModeFlags & (mask))
 
 typedef enum {
-    GPS_FIX_HOME    = (1 << 0),
+    GPS_FIX_HOME        = (1 << 0),
-    GPS_FIX         = (1 << 1),
+    GPS_FIX             = (1 << 1),
-    CALIBRATE_MAG   = (1 << 2),
+    CALIBRATE_MAG       = (1 << 2),
-    SMALL_ANGLE     = (1 << 3),
+    SMALL_ANGLE         = (1 << 3),
-    FIXED_WING      = (1 << 4),                   // set when in flying_wing or airplane mode. currently used by althold selection code
+    FIXED_WING          = (1 << 4),                   // set when in flying_wing or airplane mode. currently used by althold selection code
-    ANTI_WINDUP     = (1 << 5),
+    ANTI_WINDUP         = (1 << 5),
+    //PID_ATTENUATE       = (1 << 6),
+    FLAPERON_AVAILABLE  = (1 << 7)
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/flight/mixer.c

@@ -53,6 +53,7 @@
 
 #include "config/runtime_config.h"
 #include "config/config.h"
+#include "config/config_profile.h"
 
 //#define MIXER_DEBUG
 
@@ -355,6 +356,15 @@ void mixerUseConfigs(
 }
 
 #ifdef USE_SERVOS
+
+int16_t getFlaperonDirection(uint8_t servoPin) {
+    if (servoPin == SERVO_FLAPPERON_2) {
+        return -1;
+    } else {
+        return 1;
+    }
+}
+
 int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex)
 {
     uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel;
@@ -404,6 +414,12 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, se
         DISABLE_STATE(FIXED_WING);
     }
 
+    if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
+        ENABLE_STATE(FLAPERON_AVAILABLE);
+    } else {
+        DISABLE_STATE(FLAPERON_AVAILABLE);
+    }
+
     customMixers = initialCustomMotorMixers;
     customServoMixers = initialCustomServoMixers;
 
@@ -779,7 +795,7 @@ void mixTable(void)
 
 #ifdef USE_SERVOS
 
-void servoMixer(void)
+void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted)
 {
     int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500]
     static int16_t currentOutput[MAX_SERVO_RULES];
@@ -835,13 +851,26 @@ void servoMixer(void)
             int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2;
             int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2;
 
-            if (currentServoMixer[i].speed == 0)
+            if (currentServoMixer[i].speed == 0) {
                 currentOutput[i] = input[from];
-            else {
+            } else {
-                if (currentOutput[i] < input[from])
+                if (currentOutput[i] < input[from]) {
                     currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]);
-                else if (currentOutput[i] > input[from])
+                } else if (currentOutput[i] > input[from]) {
                     currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]);
+                }
+            }
+
+            /*
+            Flaperon fligh mode
+            */
+            if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) {
+                int8_t multiplier = 1;
+
+                if (flaperon_throw_inverted == 1) {
+                    multiplier = -1;
+                }
+                currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier;
             }
 
             servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max);

src/main/flight/mixer.h

@@ -164,6 +164,10 @@ typedef enum {
 #define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1
 #define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2
 
+#define FLAPERON_THROW_DEFAULT 250
+#define FLAPERON_THROW_MIN 100
+#define FLAPERON_THROW_MAX 400
+
 typedef struct servoMixer_s {
     uint8_t targetChannel;                  // servo that receives the output of the rule
     uint8_t inputSource;                    // input channel for this rule
@@ -235,7 +239,7 @@ int servoDirection(int servoIndex, int fromChannel);
 void mixerResetDisarmedMotors(void);
 void mixTable(void);
 void writeMotors(void);
-void servoMixer(void);
+void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted);
 void processServoTilt(void);
 void stopMotors(void);
 void StopPwmAllMotors(void);

src/main/io/rc_controls.h

@@ -51,6 +51,7 @@ typedef enum {
     BOXGCSNAV,
     BOXHEADINGLOCK,
     BOXSURFACE,
+    BOXFLAPERON,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/io/serial_cli.c

@@ -702,6 +702,8 @@ const clivalue_t valueTable[] = {
     { "yaw_jump_prevention_limit",  VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW,  YAW_JUMP_PREVENTION_LIMIT_HIGH }, 0 },
 
 #ifdef USE_SERVOS
+    { "flaperon_throw_offset",      VAR_INT16  | PROFILE_VALUE, &masterConfig.profile[0].flaperon_throw_offset, .config.minmax = { FLAPERON_THROW_MIN,  FLAPERON_THROW_MAX}, 0 },
+    { "flaperon_throw_inverted",    VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].flaperon_throw_inverted, .config.lookup = { TABLE_OFF_ON }, 0 },
     { "tri_unarmed_servo",          VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 },
     { "servo_lowpass_freq",         VAR_INT16  | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10,  400}, 0 },
     { "servo_lowpass_enable",       VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 },

src/main/io/serial_msp.c

@@ -143,6 +143,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXGCSNAV, "GCS NAV;", 31 },
     { BOXHEADINGLOCK, "HEADING LOCK;", 32 },
     { BOXSURFACE, "SURFACE;", 33 },
+    { BOXFLAPERON, "FLAPERON;", 34 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -437,8 +438,17 @@ void mspInit(void)
     }
 #endif
 
-    if (isFixedWing)
+    if (isFixedWing) {
         activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
+    }
+
+    /*
+     * FLAPERON mode active only in case of airplane and custom airplane. Activating on
+     * flying wing can cause bad thing
+     */
+    if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
+        activeBoxIds[activeBoxIdCount++] = BOXFLAPERON;
+    }
 
     activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
 
@@ -511,6 +521,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGCSNAV)) << BOXGCSNAV |
         IS_ENABLED(FLIGHT_MODE(HEADING_LOCK)) << BOXHEADINGLOCK |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
+        IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
 
     for (i = 0; i < activeBoxIdCount; i++) {

src/main/mw.c

@@ -398,6 +398,17 @@ void processRx(void)
         DISABLE_FLIGHT_MODE(HEADING_LOCK);
     }
 
+    /*
+    Flaperon mode
+    */
+    if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
+        if (!FLIGHT_MODE(FLAPERON)) {
+            ENABLE_FLIGHT_MODE(FLAPERON);
+        }
+    } else {
+        DISABLE_FLIGHT_MODE(FLAPERON);
+    }
+
 #if defined(MAG)
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
@@ -599,7 +610,7 @@ void taskMainPidLoop(void)
 #ifdef USE_SERVOS
 
     if (isMixerUsingServos()) {
-        servoMixer();
+        servoMixer(currentProfile->flaperon_throw_offset, currentProfile->flaperon_throw_inverted);
     }
 
     if (feature(FEATURE_SERVO_TILT)) {