Merge pull request #22 from Scavanger/AUTOLAND-FIX-RC1

Autoland fix rc1
breadoven · Mar 9, 2024 · 34857d1 · 34857d1
2 parents ea55fd6 + a66deda
commit 34857d1
Show file tree

Hide file tree

Showing 10 changed files with 87 additions and 67 deletions.
diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h
@@ -105,6 +105,7 @@ typedef enum {
     TURTLE_MODE           = (1 << 15),
     SOARING_MODE          = (1 << 16),
     ANGLEHOLD_MODE        = (1 << 17),
+    NAV_FW_AUTOLAND       = (1 << 18)
 } flightModeFlags_e;
 
 extern uint32_t flightModeFlags;

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -594,7 +594,7 @@ static float computePidLevelTarget(flight_dynamics_index_t axis) {
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
 #ifdef USE_FW_AUTOLAND
-    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !isFwLandInProgess()) {
+    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && ! FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 #else
     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
 #endif

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
@@ -985,7 +985,7 @@ static const char * osdFailsafePhaseMessage(void)
 
 static const char * osdFailsafeInfoMessage(void)
 {
-    if (failsafeIsReceivingRxData()) {
+    if (failsafeIsReceivingRxData() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
         // User must move sticks to exit FS mode
         return OSD_MESSAGE_STR(OSD_MSG_MOVE_EXIT_FS);
     }
@@ -2277,7 +2277,7 @@ static bool osdDrawSingleElement(uint8_t item)
         {
             char *p = "ACRO";
 #ifdef USE_FW_AUTOLAND
-            if (isFwLandInProgess()) 
+            if (FLIGHT_MODE(NAV_FW_AUTOLAND)) 
                 p = "LAND";
             else
 #endif
@@ -5142,7 +5142,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
         if (ARMING_FLAG(ARMED)) {
 #ifdef USE_FW_AUTOLAND
-            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
+            if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || FLIGHT_MODE(NAV_FW_AUTOLAND)) {
                 if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) {
 #else
             if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
@@ -5183,7 +5183,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 #ifdef USE_FW_AUTOLAND
                     if (canFwLandCanceld()) {
                          messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
-                    } else if (!isFwLandInProgess()) {
+                    } else if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 #endif
                         const char *navStateMessage = navigationStateMessage();
                         if (navStateMessage) {

diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
@@ -1059,6 +1059,11 @@ static bool djiFormatMessages(char *buff)
                 if (FLIGHT_MODE(MANUAL_MODE)) {
                     messages[messageCount++] = "(MANUAL)";
                 }
+
+                if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
+                     messages[messageCount++] = "(LAND)";
+                }
+
             }
         }
         // Pick one of the available messages. Each message lasts

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -693,7 +693,6 @@ uint8_t getActiveWpNumber(void);
 int32_t navigationGetHomeHeading(void);
 
 #ifdef USE_FW_AUTOLAND
-bool isFwLandInProgess(void);
 bool canFwLandCanceld(void);
 #endif
 

diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
@@ -644,7 +644,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
 
         // Manual throttle increase
 #ifdef USE_FW_AUTOLAND
-        if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) {
+        if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 #else
         if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) {
 #endif

diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c
@@ -746,7 +746,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
 #ifdef USE_FW_AUTOLAND
-            return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0;
+            return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || FLIGHT_MODE(NAV_FW_AUTOLAND)) ? 1 : 0;
 #else
             return ((navGetCurrentStateFlags() & NAV_CTL_LAND)) ? 1 : 0;
 #endif

diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
@@ -371,6 +371,11 @@ static void crsfFrameFlightMode(sbuf_t *dst)
         } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
             flightMode = "ANGH";
         }
+#ifdef USE_FW_AUTOLAND
+    } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
+        flightMode = "LAND"
+    }
+#endif
 #ifdef USE_GPS
     } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
         flightMode = "WAIT"; // Waiting for GPS lock

diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
@@ -178,6 +178,10 @@ void ltm_sframe(sbuf_t *dst)
         lt_flightmode = LTM_MODE_ANGLE;
     else if (FLIGHT_MODE(HORIZON_MODE))
         lt_flightmode = LTM_MODE_HORIZON;
+#ifdef USE_FW_AUTOLAND
+    else if (FLIGHT_MODE(NAV_FW_AUTOLAND))
+        lt_flightmode = LTM_MODE_LAND;
+#endif
     else
         lt_flightmode = LTM_MODE_RATE;      // Rate mode