On MR allow airmode to be sticky and activated by throttle threshold (#4634)

* on MR allow airmode to be sticky and activated by throttle threshold

* fix bug

* make airmode activation threshold configurable

* Keep arimore activation flag as state

* Review changes

src/main/fc/fc_core.c

@@ -446,7 +446,7 @@ void processRx(timeUs_t currentTimeUs)
     const throttleStatus_e throttleStatus = calculateThrottleStatus();
 
     processRcStickPositions(throttleStatus);
-
+    processAirmode();
     updateActivatedModes();
 
 #ifdef USE_PINIOBOX
@@ -550,9 +550,9 @@ void processRx(timeUs_t currentTimeUs)
         /* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
         pidResetErrorAccumulators();
     }
-    else {
+    else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
         if (throttleStatus == THROTTLE_LOW) {
-            if (isAirmodeActive() && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
+            if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
                 rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
 
                 // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
@@ -571,6 +571,12 @@ void processRx(timeUs_t currentTimeUs)
         else {
             DISABLE_STATE(ANTI_WINDUP);
         }
+    } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
+        DISABLE_STATE(ANTI_WINDUP);
+        //This case applies only to MR when Airmode management is throttle threshold activated
+        if (throttleStatus == THROTTLE_LOW && !STATE(AIRMODE_ACTIVE)) {
+            pidResetErrorAccumulators();
+        }
     }
 
     if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {

src/main/fc/rc_controls.c

@@ -69,14 +69,16 @@ stickPositions_e rcStickPositions;
 
 FASTRAM int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1);
 
 PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
     .deadband = 5,
     .yaw_deadband = 5,
     .pos_hold_deadband = 20,
     .alt_hold_deadband = 50,
-    .deadband3d_throttle = 50
+    .deadband3d_throttle = 50,
+    .airmodeHandlingType = STICK_CENTER,
+    .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD,
 );
 
 PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2);

src/main/fc/rc_controls.h

@@ -19,6 +19,8 @@
 
 #include "config/parameter_group.h"
 
+#define AIRMODE_THROTTLE_THRESHOLD 1300
+
 typedef enum rc_alias {
     ROLL = 0,
     PITCH,
@@ -48,6 +50,11 @@ typedef enum {
     CENTERED
 } rollPitchStatus_e;
 
+typedef enum {
+    STICK_CENTER = 0,
+    THROTTLE_THRESHOLD
+} airmodeAndAntiWindupHandlingType_e;
+
 typedef enum {
     ROL_LO = (1 << (2 * ROLL)),
     ROL_CE = (3 << (2 * ROLL)),
@@ -74,6 +81,8 @@ typedef struct rcControlsConfig_s {
     uint8_t pos_hold_deadband;              // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
     uint8_t alt_hold_deadband;              // Defines the neutral zone of throttle stick during altitude hold
     uint16_t deadband3d_throttle;           // default throttle deadband from MIDRC
+    uint8_t airmodeHandlingType;            // Defaults to ANTI_WINDUP triggered at sticks centered
+    uint16_t airmodeThrottleThreshold;      // Throttle threshold for airmode initial activation
 } rcControlsConfig_t;
 
 PG_DECLARE(rcControlsConfig_t, rcControlsConfig);

src/main/fc/rc_modes.c

@@ -31,6 +31,7 @@
 
 #include "fc/config.h"
 #include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
 
 #include "rx/rx.h"
 
@@ -53,9 +54,62 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
 PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
 PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
 
-bool isAirmodeActive(void)
-{
-    return feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
+static void processAirmodeAirplane(void) {
+    if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+        ENABLE_STATE(AIRMODE_ACTIVE);
+    } else {
+        DISABLE_STATE(AIRMODE_ACTIVE);
+    }
+}
+
+static void processAirmodeMultirotor(void) {
+    if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
+        if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
+            ENABLE_STATE(AIRMODE_ACTIVE);
+        } else {
+            DISABLE_STATE(AIRMODE_ACTIVE);
+        }
+    } else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
+
+        if (!ARMING_FLAG(ARMED)) {
+            /*
+             * Disarm disables airmode immediately
+             */
+            DISABLE_STATE(AIRMODE_ACTIVE);
+        } else if (
+            !STATE(AIRMODE_ACTIVE) && 
+            rcCommand[THROTTLE] > rcControlsConfig()->airmodeThrottleThreshold &&
+            (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE))
+        ) {
+            /*
+             * Airmode is allowed to be active only after ARMED and then THROTTLE goes above
+             * activation threshold
+             */
+            ENABLE_STATE(AIRMODE_ACTIVE);
+        } else if (
+            STATE(AIRMODE_ACTIVE) &&
+            !feature(FEATURE_AIRMODE) &&
+            !IS_RC_MODE_ACTIVE(BOXAIRMODE)
+        ) {
+            /*
+             *  When user disables BOXAIRMODE, turn airmode off as well
+             */
+            DISABLE_STATE(AIRMODE_ACTIVE);
+        }
+
+    } else {
+        DISABLE_STATE(AIRMODE_ACTIVE);
+    }
+}
+
+void processAirmode(void) {
+
+    if (STATE(FIXED_WING)) {
+        processAirmodeAirplane();
+    } else {
+        processAirmodeMultirotor();
+    }
+
 }
 
 #if defined(USE_NAV)

src/main/fc/rc_modes.h

@@ -120,7 +120,7 @@ void rcModeUpdate(boxBitmask_t *newState);
 
 bool isModeActivationConditionPresent(boxId_e modeId);
 
-bool isAirmodeActive(void);
+void processAirmode(void);
 bool isUsingNavigationModes(void);
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 

src/main/fc/runtime_config.h

@@ -104,6 +104,7 @@ typedef enum {
     NAV_CRUISE_BRAKING_BOOST            = (1 << 12),
     NAV_CRUISE_BRAKING_LOCKED           = (1 << 13),
     NAV_EXTRA_ARMING_SAFETY_BYPASSED    = (1 << 14),    // nav_extra_arming_safey was bypassed. Keep it until power cycle.
+    AIRMODE_ACTIVE                      = (1 << 15),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/fc/settings.yaml

@@ -120,6 +120,8 @@ tables:
   - name: iterm_relax_type
     values: ["GYRO", "SETPOINT"]
     enum: itermRelaxType_e
+  - name: airmodeHandlingType
+    values: ["STICK_CENTER", "THROTTLE_THRESHOLD"]
   - name: nav_extra_arming_safety
     values: ["OFF", "ON", "ALLOW_BYPASS"]
     enum: navExtraArmingSafety_e
@@ -851,6 +853,13 @@ groups:
         field: deadband3d_throttle
         min: 0
         max: 200
+      - name: mc_airmode_type
+        field: airmodeHandlingType
+        table: airmodeHandlingType
+      - name: mc_airmode_threshold
+        field: airmodeThrottleThreshold
+        min: 1000
+        max: 2000
 
   - name: PG_PID_PROFILE
     type: pidProfile_t

src/main/flight/mixer.c

@@ -404,7 +404,7 @@ motorStatus_e getMotorStatus(void)
     }
 
     if (rxGetChannelValue(THROTTLE) < rxConfig()->mincheck) {
-        if ((STATE(FIXED_WING) || !isAirmodeActive()) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
+        if ((STATE(FIXED_WING) || !STATE(AIRMODE_ACTIVE)) && (!(navigationIsFlyingAutonomousMode() && navConfig()->general.flags.auto_overrides_motor_stop)) && (!failsafeIsActive())) {
             return MOTOR_STOPPED_USER;
         }
     }

src/main/io/osd.c

@@ -1597,7 +1597,7 @@ static bool osdDrawSingleElement(uint8_t item)
                 p = "ANGL";
             else if (FLIGHT_MODE(HORIZON_MODE))
                 p = "HOR ";
-            else if (isAirmodeActive())
+            else if (STATE(AIRMODE_ACTIVE))
                 p = "AIR ";
 
             displayWrite(osdDisplayPort, elemPosX, elemPosY, p);

src/main/telemetry/crsf.c

@@ -296,7 +296,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
     // use same logic as OSD, so telemetry displays same flight text as OSD when armed
     const char *flightMode = "OK";
     if (ARMING_FLAG(ARMED)) {
-        if (isAirmodeActive()) {
+        if (STATE(AIRMODE_ACTIVE)) {
             flightMode = "AIR";
         } else {
             flightMode = "ACRO";

src/test/unit/flight_failsafe_unittest.cc.txt

@@ -458,9 +458,4 @@ uint16_t getCurrentMinthrottle(void)
     return testMinThrottle;
 }
 
-bool isUsingSticksForArming(void)
-{
-    return isUsingSticksToArm;
-}
-
 }