Merge pull request #10621 from breadoven/abo_fw_flight_detect_fix

Fixed wing flight detection fix
iNavFlight · Jan 25, 2025 · b0dd438 · b0dd438
2 parents 1c9cac5 + 0d9dd1b
commit b0dd438
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 4 deletions.
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -3543,6 +3543,7 @@ void updateLandingStatus(timeMs_t currentTimeMs)
             landingDetectorIsActive = false;
         }
         resetLandingDetector();
+        getTakeoffAltitude();
 
         return;
     }
@@ -5278,6 +5279,16 @@ uint8_t getActiveWpNumber(void)
     return NAV_Status.activeWpNumber;
 }
 
+float getTakeoffAltitude(void)
+{
+    static float refTakeoffAltitude = 0.0f;
+    if (!ARMING_FLAG(ARMED) && !landingDetectorIsActive) {
+        refTakeoffAltitude = posControl.actualState.abs.pos.z;
+    }
+
+    return refTakeoffAltitude;
+}
+
 #ifdef USE_FW_AUTOLAND
 
 static void resetFwAutoland(void)

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
@@ -712,10 +712,11 @@ bool isFixedWingFlying(void)
     }
 #endif
     bool throttleCondition = getMotorCount() == 0 || rcCommand[THROTTLE] > currentBatteryProfile->nav.fw.cruise_throttle;
-    bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f;
+    bool velCondition = posControl.actualState.velXY > 350.0f || airspeed > 350.0f;
+    bool altCondition = fabsf(posControl.actualState.abs.pos.z - getTakeoffAltitude()) > 500.0f;
     bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING;
 
-    return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition;
+    return (isGPSHeadingValid() && throttleCondition && velCondition && altCondition) || launchCondition;
 }
 
 /*-----------------------------------------------------------

diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c
@@ -299,7 +299,7 @@ static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
 
 static inline bool isLaunchMaxAltitudeReached(void)
 {
-    return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
+    return (navConfig()->fw.launch_max_altitude > 0) && ((getEstimatedActualPosition(Z) - getTakeoffAltitude()) >= navConfig()->fw.launch_max_altitude);
 }
 
 static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)

diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
@@ -423,7 +423,7 @@ typedef enum {
 #ifdef USE_GEOZONE
 typedef struct navSendTo_s {
     fpVector3_t targetPos;
-    uint16_t altitudeTargetRange;   // 0 for only "2D" 
+    uint16_t altitudeTargetRange;   // 0 for only "2D"
     uint32_t targetRange;
     bool lockSticks;
     uint32_t lockStickTime;
@@ -554,6 +554,7 @@ bool isNavHoldPositionActive(void);
 bool isLastMissionWaypoint(void);
 float getActiveSpeed(void);
 bool isWaypointNavTrackingActive(void);
+float getTakeoffAltitude(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse);
 void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);