[NAV/MIXER] Refactor getMotorStop() code; Avoid recursion and crash when nav_overrides_motor_stop is set to NOMS_ALL_NAV

docs/Settings.md

@@ -263,6 +263,7 @@
 | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
 | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
 | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] |
+| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
 | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
 | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
 | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] |

src/main/flight/mixer.c

@@ -574,15 +574,32 @@ void FAST_CODE mixTable(const float dT)
 
 motorStatus_e getMotorStatus(void)
 {
-    if (failsafeRequiresMotorStop() || (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE))) {
+    if (failsafeRequiresMotorStop()) {
         return MOTOR_STOPPED_AUTO;
     }
 
-    if (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW) {
+    if (!failsafeIsActive() && STATE(NAV_MOTOR_STOP_OR_IDLE)) {
-        navOverridesMotorStop_e motorStopOverride = navConfig()->general.flags.nav_overrides_motor_stop;
+        return MOTOR_STOPPED_AUTO;
-        if (!failsafeIsActive() && (STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE)) &&
+    }
-            ((motorStopOverride == NOMS_OFF) || ((motorStopOverride == NOMS_ALL_NAV) && !navigationIsControllingThrottle()) || ((motorStopOverride == NOMS_AUTO_ONLY) && !navigationIsFlyingAutonomousMode()))) {
+
-            return MOTOR_STOPPED_USER;
+    const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
+    const bool throttleStickLow =
+        (calculateThrottleStatus(feature(FEATURE_REVERSIBLE_MOTORS) ? THROTTLE_STATUS_TYPE_COMMAND : THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW);
+
+    if (throttleStickLow && fixedWingOrAirmodeNotActive && !failsafeIsActive()) {
+        // If user is holding stick low, we are not in failsafe and either on a plane or on a quad with inactive
+        // airmode - we need to check if we are allowing navigation to override MOTOR_STOP
+
+        switch (navConfig()->general.flags.nav_overrides_motor_stop) {
+            case NOMS_ALL_NAV:
+                return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
+
+            case NOMS_AUTO_ONLY:
+                return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
+
+            case NOMS_OFF:
+            default:
+                return MOTOR_STOPPED_USER;
         }
     }
 

src/main/navigation/navigation.c

@@ -3649,10 +3649,16 @@ bool navigationIsExecutingAnEmergencyLanding(void)
     return navGetCurrentStateFlags() & NAV_CTL_EMERG;
 }
 
-bool navigationIsControllingThrottle(void)
+bool navigationInAutomaticThrottleMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
-    return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER));
+    return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
+}
+
+bool navigationIsControllingThrottle(void)
+{
+    // Note that this makes a detour into mixer code to evaluate actual motor status
+    return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER);
 }
 
 bool navigationIsFlyingAutonomousMode(void)

src/main/navigation/navigation.h

@@ -513,6 +513,7 @@ void abortForcedRTH(void);
 rthState_e getStateOfForcedRTH(void);
 
 /* Getter functions which return data about the state of the navigation system */
+bool navigationInAutomaticThrottleMode(void);
 bool navigationIsControllingThrottle(void);
 bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);