Use wasThrottleRaised in place of isAirmodeActivated where appropriate (#13968)

* use wasThrottleRaised in place of isAirmodeActivated where airmode isn't really needed

* fix unit test

* fix typo

* remove unnecessary check

* final changes and clarificartions

* Update src/main/flight/mixer.c

Co-authored-by: Ivan Efimov <gendalf44@yandex.ru>

---------

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
Co-authored-by: Ivan Efimov <gendalf44@yandex.ru>

src/main/fc/core.c

@@ -202,7 +202,7 @@ bool canUseLaunchControl(void)
     if (!isFixedWing()
         && !isUsingSticksForArming()     // require switch arming for safety
         && IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)
-        && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled())  // can't use when motors are stopped
+        && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled()) // either not using motor_stop, or motor_stop is blocked by Airmode
         && !featureIsEnabled(FEATURE_3D) // pitch control is not 3D aware
         && (flightModeFlags == 0)) {     // don't want to use unless in acro mode
         return true;
@@ -769,14 +769,13 @@ uint8_t calculateThrottlePercentAbs(void)
     return abs(calculateThrottlePercent());
 }
 
-static bool airmodeIsActivated;
+static bool throttleRaised = false;
 
-bool isAirmodeActivated(void)
+bool wasThrottleRaised(void)
 {
-    return airmodeIsActivated;
+    return throttleRaised;
 }
 
-
 /*
  * processRx called from taskUpdateRxMain
  */
@@ -803,17 +802,23 @@ bool processRx(timeUs_t currentTimeUs)
     const bool throttleActive = calculateThrottleStatus() != THROTTLE_LOW;
     const uint8_t throttlePercent = calculateThrottlePercentAbs();
     const bool launchControlActive = isLaunchControlActive();
+    static bool isAirmodeActive;
 
-    if (airmodeIsEnabled() && ARMING_FLAG(ARMED) && !launchControlActive) {
+    if (ARMING_FLAG(ARMED)) {
-        // once throttle exceeds activate threshold, airmode latches active until disarm
         if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
-            airmodeIsActivated = true;
+            throttleRaised = true; // Latch true until disarm
+        }
+        if (isAirmodeEnabled() && !launchControlActive) {
+            isAirmodeActive = throttleRaised;
         }
     } else {
-        airmodeIsActivated = false;
+        throttleRaised = false;
+        isAirmodeActive = false;
     }
 
-    if (ARMING_FLAG(ARMED) && (airmodeIsActivated || throttleActive || launchControlActive || isFixedWing())) {
+    // Note: If Airmode is enabled, on arming, iTerm and PIDs will be off until throttle exceeds the threshold (OFF while disarmed)
+    // If not, iTerm will be off at low throttle, with pidStabilisationState determining whether PIDs will be active
+    if (ARMING_FLAG(ARMED) && (isAirmodeActive || throttleActive || launchControlActive || isFixedWing())) {
         pidSetItermReset(false);
         pidStabilisationState(PID_STABILISATION_ON);
     } else {
@@ -841,7 +846,7 @@ bool processRx(timeUs_t currentTimeUs)
         //   - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
         //   - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
         bool inStableFlight = false;
-        if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || throttleActive) { // are motors running?
+        if (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled() || throttleActive) { // are motors running?
             const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
             const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
             if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
@@ -931,7 +936,7 @@ void processRxModes(timeUs_t currentTimeUs)
         && featureIsEnabled(FEATURE_MOTOR_STOP)
         && !isFixedWing()
         && !featureIsEnabled(FEATURE_3D)
-        && !airmodeIsEnabled()
+        && !isAirmodeEnabled()
         && !FLIGHT_MODE(GPS_RESCUE_MODE)  // disable auto-disarm when GPS Rescue is active
     ) {
         if (isUsingSticksForArming()) {
@@ -1025,8 +1030,8 @@ void processRxModes(timeUs_t currentTimeUs)
         && sensors(SENSOR_ACC)
         // and we have altitude data
         && isAltitudeAvailable()
-        // and we have already taken off (to prevent activation on the ground), then enable althold
+        // prevent activation until after takeoff
-        && isAirmodeActivated()) {
+        && wasThrottleRaised()) {
         if (!FLIGHT_MODE(ALT_HOLD_MODE)) {
             ENABLE_FLIGHT_MODE(ALT_HOLD_MODE);
         }
@@ -1162,7 +1167,8 @@ static FAST_CODE_NOINLINE void subTaskPidController(timeUs_t currentTimeUs)
         && !crashFlipModeActive
         && !runawayTakeoffTemporarilyDisabled
         && !FLIGHT_MODE(GPS_RESCUE_MODE)   // disable Runaway Takeoff triggering if GPS Rescue is active
-        && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) {
+        // check that motors are running
+        && (!featureIsEnabled(FEATURE_MOTOR_STOP) || isAirmodeEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) {
 
         if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
             || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)

src/main/fc/core.h

@@ -91,7 +91,7 @@ int8_t calculateThrottlePercent(void);
 uint8_t calculateThrottlePercentAbs(void);
 bool areSticksActive(uint8_t stickPercentLimit);
 void runawayTakeoffTemporaryDisable(uint8_t disableFlag);
-bool isAirmodeActivated();
+bool wasThrottleRaised();
 timeUs_t getLastDisarmTimeUs(void);
 bool isTryingToArm();
 void resetTryingToArm();

src/main/fc/rc_modes.c

@@ -72,7 +72,7 @@ void rcModeUpdate(const boxBitmask_t *newState)
     rcModeActivationMask = *newState;
 }
 
-bool airmodeIsEnabled(void)
+bool isAirmodeEnabled(void)
 {
     return airmodeEnabled;
 }

src/main/fc/rc_modes.h

@@ -142,7 +142,7 @@ typedef struct modeActivationProfile_s {
 bool IS_RC_MODE_ACTIVE(boxId_e boxId);
 void rcModeUpdate(const boxBitmask_t *newState);
 
-bool airmodeIsEnabled(void);
+bool isAirmodeEnabled(void);
 
 bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
 void updateActivatedModes(void);

src/main/flight/mixer.c

@@ -228,7 +228,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
         currentThrottleInputRange = PWM_RANGE;
 #ifdef USE_DYN_IDLE
         if (mixerRuntime.dynIdleMinRps > 0.0f) {
-            const float maxIncrease = isAirmodeActivated()
+            const float maxIncrease = wasThrottleRaised()
                 ? mixerRuntime.dynIdleMaxIncrease : mixerRuntime.dynIdleStartIncrease;
             float minRps = getMinMotorFrequencyHz();
             DEBUG_SET(DEBUG_DYN_IDLE, 3, lrintf(minRps * 10.0f));
@@ -679,6 +679,9 @@ static void applyMixerAdjustment(float *motorMix, const float motorMixMin, const
 
 FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
 {
+    const bool launchControlActive = isLaunchControlActive();
+    const bool airmodeEnabled = isAirmodeEnabled() || launchControlActive;
+
     // Find min and max throttle based on conditions. Throttle has to be known before mixing
     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 
@@ -686,8 +689,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
         return; // if crash flip mode has been applied to the motors, mixing is done
     }
 
-    const bool launchControlActive = isLaunchControlActive();
-
     motorMixer_t * activeMixer = &mixerRuntime.currentMixer[0];
 #ifdef USE_LAUNCH_CONTROL
     if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
@@ -781,8 +782,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
     }
 
     //  The following fixed throttle values will not be shown in the blackbox log
-    // ?? Should they be influenced by airmode?  If not, should go after the apply airmode code.
-    const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
 #ifdef USE_YAW_SPIN_RECOVERY
     // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
     // When airmode is active the throttle setting doesn't impact recovery authority.
@@ -816,6 +815,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
 
     motorMixRange = motorMixMax - motorMixMin;
 
+    // note that here airmodeEnabled is true also when Launch Control is active
     switch (mixerConfig()->mixer_type) {
     case MIXER_LEGACY:
         applyMixerAdjustment(motorMix, motorMixMin, motorMixMax, airmodeEnabled);
@@ -835,10 +835,9 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
     if (featureIsEnabled(FEATURE_MOTOR_STOP)
         && ARMING_FLAG(ARMED)
         && !mixerRuntime.feature3dEnabled
-        && !airmodeEnabled
+        && !airmodeEnabled                               // not with airmode or launch control
-        && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE)   // disable motor_stop while GPS Rescue / Altitude Hold is active
+        && !FLIGHT_MODE(GPS_RESCUE_MODE | ALT_HOLD_MODE) // not in autopilot modes
         && (rcData[THROTTLE] < rxConfig()->mincheck)) {
-        // motor_stop handling
         applyMotorStop();
     } else {
         // Apply the mix to motor endpoints

src/main/flight/pid.c

@@ -758,7 +758,7 @@ STATIC_UNIT_TESTED void applyAbsoluteControl(const int axis, const float gyroRat
             acErrorRate = (gyroRate > gmaxac ? gmaxac : gminac ) - gyroRate;
         }
 
-        if (isAirmodeActivated()) {
+        if (wasThrottleRaised()) {
             axisError[axis] = constrainf(axisError[axis] + acErrorRate * pidRuntime.dT,
                 -pidRuntime.acErrorLimit, pidRuntime.acErrorLimit);
             const float acCorrection = constrainf(axisError[axis] * pidRuntime.acGain, -pidRuntime.acLimit, pidRuntime.acLimit);
@@ -840,8 +840,8 @@ float pidGetAirmodeThrottleOffset(void)
 
 static FAST_CODE_NOINLINE void disarmOnImpact(void)
 {
-    // if, after takeoff...
+    // if, being armed, and after takeoff...
-    if (isAirmodeActivated()
+    if (wasThrottleRaised()
         // and, either sticks are centred and throttle zeroed,
         && ((getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f)
             // we could test here for stage 2 failsafe (including both landing or GPS Rescue)

src/main/osd/osd_elements.c

@@ -1066,7 +1066,7 @@ static void osdElementFlymode(osdElementParms_t *element)
         strcpy(element->buff, "HOR ");
     } else if (IS_RC_MODE_ACTIVE(BOXACROTRAINER)) {
         strcpy(element->buff, "ATRN");
-    } else if (airmodeIsEnabled()) {
+    } else if (isAirmodeEnabled()) {
         strcpy(element->buff, "AIR ");
     } else {
         strcpy(element->buff, "ACRO");

src/main/telemetry/crsf.c

@@ -405,7 +405,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
         flightMode = "ALTH";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";
-    } else if (airmodeIsEnabled()) {
+    } else if (isAirmodeEnabled()) {
         flightMode = "AIR";
     }
 

src/test/unit/osd_unittest.cc

@@ -1314,7 +1314,7 @@ extern "C" {
         return false;
     }
 
-    bool airmodeIsEnabled() {
+    bool isAirmodeEnabled() {
         return false;
     }
 

src/test/unit/pid_unittest.cc

@@ -24,7 +24,7 @@
 #include "gtest/gtest.h"
 #include "build/debug.h"
 
-bool simulatedAirmodeEnabled = true;
+bool simulatedThrottleRaised = true;
 float simulatedSetpointRate[3] = { 0,0,0 };
 float simulatedPrevSetpointRate[3] = { 0,0,0 };
 float simulatedRcDeflection[3] = { 0,0,0 };
@@ -93,7 +93,7 @@ extern "C" {
 
     float getMotorMixRange(void) { return simulatedMotorMixRange; }
     float getSetpointRate(int axis) { return simulatedSetpointRate[axis]; }
-    bool isAirmodeActivated(void) { return simulatedAirmodeEnabled; }
+    bool wasThrottleRaised(void) { return simulatedThrottleRaised; }
     float getRcDeflectionAbs(int axis) { return fabsf(simulatedRcDeflection[axis]); }
 
     // used by auto-disarm code

src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc

@@ -420,7 +420,7 @@ extern "C" {
     bool telemetryIsSensorEnabled(sensor_e) {return true; }
     bool sensors(uint32_t ) { return true; }
 
-    bool airmodeIsEnabled(void) {return airMode; }
+    bool isAirmodeEnabled(void) {return airMode; }
 
     bool isBatteryVoltageConfigured(void) { return true; }
     bool isAmperageConfigured(void) { return true; }

src/test/unit/telemetry_crsf_msp_unittest.cc

@@ -292,7 +292,7 @@ extern "C" {
 
     bool featureIsEnabled(uint32_t) {return false;}
 
-    bool airmodeIsEnabled(void) {return true;}
+    bool isAirmodeEnabled(void) {return true;}
 
     mspDescriptor_t mspDescriptorAlloc(void) {return 0;}
 

src/test/unit/telemetry_crsf_unittest.cc

@@ -343,7 +343,7 @@ bool telemetryIsSensorEnabled(sensor_e) {return true;}
 
 portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}
 
-bool airmodeIsEnabled(void) {return airMode;}
+bool isAirmodeEnabled(void) {return airMode;}
 
 int32_t getAmperage(void)
 {