Fix Nav Launch detection related bugs

src/main/flight/rth_estimator.c

@@ -145,21 +145,6 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee
 
 // returns Wh
 static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) {
-    // Fixed wing only for now
-    if (!STATE(FIXED_WING_LEGACY))
-        return -1;
-
-    if (!(feature(FEATURE_VBAT) && batteryWasFullWhenPluggedIn()
-          && feature(FEATURE_CURRENT_METER) && (batteryMetersConfig()->cruise_power > 0)
-          && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0)
-          && navigationPositionEstimateIsHealthy() && isImuHeadingValid() && (navConfig()->fw.cruise_speed > 0)
-          && ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && isFixedWingLaunchDetected()))
-          && (ARMING_FLAG(ARMED))
-#ifdef USE_WIND_ESTIMATOR
-          && (!takeWindIntoAccount || isEstimatedWindSpeedValid())
-#endif
-       ))
-        return -1;
 
     const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100);
 
@@ -224,6 +209,26 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) {
 // returns meters
 float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) {
 
+    // Fixed wing only for now
+    if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) {
+        return -1;
+    }
+
+#ifdef USE_WIND_ESTIMATOR
+    if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) {
+        return -1;
+    }
+#endif
+
+    // check requirements
+    const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn();
+    const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &&currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0;
+    const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid();
+
+    if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) {
+        return -1;
+    }
+
     const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount);
 
     // error: return error code directly

src/main/navigation/navigation_fw_launch.c

@@ -388,10 +388,6 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
         return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state
     }
 
-    if (isLaunchMaxAltitudeReached()) {
-        return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
-    }
-
     if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
         return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
     }
@@ -453,7 +449,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs)
 
 bool isFixedWingLaunchDetected(void)
 {
-    return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
+    return fwLaunch.currentState >= FW_LAUNCH_STATE_DETECTED;
 }
 
 void enableFixedWingLaunchController(timeUs_t currentTimeUs)