Improvements

docs/Fixed Wing Landing.md

@@ -0,0 +1,91 @@
+# Fixed Wing Landing
+
+## Introducion
+
+INAV supports advanced automatic landings for fixed wing aircraft from version 8.
+The procedure is based on landings for man-carrying aircraft, so that safe landings at a specific location are possible.
+Supported are landings at the safehome after "Return to Home" or at a defined LAND waypoint for missions. 
+
+## General procedure:
+
+1. After reaching Safehome/LAND Waypoint the altitude is corrected to "Approach Altitude".
+2. The aircraft circles for at least 30 seconds to determine the wind direction and strength.
+3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2.
+4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held.
+5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude".
+6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is reduced to "Land Altitude".
+7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held.
+7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held
+8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting nav_disarm_on_landing
+
+The following graphics illustrate the process:
+
+## Landing site parameters:
+
+### The following parameters are set for each landing site (Safefome/LAND waypoint):
+
+All settings can also be conveniently made in the Configurator via Missionplanner
+
+CLI command fwapproach:
+
+fwapproach <index> <Apprach direction> <Approach altitude> <Land altitude> <approach heading 1> <approach heading 2> <sea level>
+
+fwapprach has 17 slots in which landing parameters can be stored. In slot 0-7 the landing parameters for Safehome are stored, in 8 - 16 the parameters for waypoint missions. Only one landing point per mission can be saved. 
+
+- index: 0 - 17, 0 - 7 Safehome, 8 - 16 Mission
+- Approach direction: 0 - Left, 1 - Right. Always seen from the primary landing direction (positive value), i.e. whether the aircraft flies left or right turns on approach.
+- Approach Altitude: Initial altitude of the approach, the altitude at which the wind direction is determined and the downwind approach, in cm
+- Land Altitude: Altitude of the landing site, in cm
+- Approach heading 1 and 2: Two landing directions can be set, values: 0 - +/-360. 0 = landing direction is deactivated. 
+A positive value means that you can approach in both directions, a negative value means that this direction is exclusive.
+Example: 90 degrees: It is possible to land in 90 degrees as well as in 270 degrees. -90 means that you can only land in a 90 degree direction.
+This means that practically 4 landing directions can be saved.
+- Sea Level: 0 - Deactivated, 1 - Activated. If activated, approach and land altitude refer to normal zero (sea level), otherwise relative altitude to the altitude during first GPS fix.
+WARNING: The Configuator automatically determines the ground altitude based on databases on the Internet, which may be inaccurate. Please always compare with the measured GPS altitude at the landing site to avoid crashes.
+
+### Global paramters
+
+All settings are available via “Advanced Tuning” in the Configurator.
+
+- nav_fw_land_approach_length: Length of the final approach, measured from the land site (Safehome/Waypoint) to the last turning point.
+  In cm. Max: 100000, Min: 100, Default: 35000
+
+- nav_fw_land_final_approach_pitch2throttle_mod: Modifier for pitch to throttle ratio at final approach. This parameter can be used to reduce speed during the final approach. 
+Example: If the parameter is set to 200% and Pitch To Throttle Ratio is set to 15, Pitch To Throttle Ratio is set to 30 on the final approach. This causes a reduction in engine power on approach when the nose is pointing downwards.
+  In Percent. Min: 100, Max: 400, Default: 100
+
+- nav_fw_land_glide_alt: Initial altitude of the glide phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
+  In cm. Min: 100, Max: 5000, Default: 200
+
+- nav_fw_land_flare_alt: Initial altitude of the flare phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
+  In cm. Min: 0, Max: 5000, Default: 200
+
+- nav_fw_land_glide_pitch: Pitch value for glide phase. 
+  In degrees. Min: 0, Max: 45, Default: 0
+
+- nav_fw_land_flare_pitch: Pitch value for flare phase. 
+  In degrees. Min: 0, Max: 45, Default: 8
+
+- nav_fw_land_max_tailwind: Max. tailwind if no landing direction with downwind is available. Wind strengths below this value are   ignored  (error tolerance in the wind measurement). Landing then takes place in the main direction. If, for example, 90 degrees is configured, landing takes place in this direction, NOT in 270 degrees (see above).
+ In cm/s. Min: 0; Max: 3000, Default: 140
+
+## Waypoint missions
+
+Only one landing waypoint per mission can be active and saved and the landing waypoint must be the last waypoint of the mission. 
+If the altitude of the waypoint and the "Approach Altitude" are different, the altitude of the waypoint is approached first and then the altitude is corrected to "Approach Altitude".
+
+## Logic Conditions
+
+The current landing state can be retrieved via ID 38 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deplyoment of the landing flaps.
+
+### The returned values:
+ 0: Idle/Inactive
+ 1: Downwind
+ 2: Base Leg
+ 3: Final Approach
+ 4: Glide
+ 5: Flare
+
+## Tuning Tipps:
+
+ To do

docs/Programming Framework.md

@@ -156,6 +156,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
 | 38            | ACTIVE_MIXER_PROFILE          | Which mixers are currently active (for vtol etc) |
 | 39            | MIXER_TRANSITION_ACTIVE       | Currently switching between mixers (quad to plane etc) |
 | 40            | ATTITUDE_YAW                  | current heading (yaw) in `degrees` |
+| 38            | FW Land Sate               | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
 
 #### FLIGHT_MODE
 

src/main/fc/cli.c

@@ -1311,7 +1311,7 @@ static void cliTempSensor(char *cmdline)
 
 static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach) 
 {
-    const char *format = "fwapproach %u %d %d %d %u %d %d %u";
+    const char *format = "fwapproach %u %d %d %u %d %d %u";
     for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
         bool equalsDefault = false;
         if (defaultFwAutolandApproach) {
@@ -1334,7 +1334,7 @@ static void cliFwAutolandApproach(char * cmdline)
      if (isEmpty(cmdline)) {
         printFwAutolandApproach(DUMP_MASTER, fwAutolandApproachConfig(0), NULL);
     } else if (sl_strcasecmp(cmdline, "reset") == 0) {
-        resetFwAutolandApproach();
+        resetFwAutolandApproach(-1);
     } else {
         int32_t approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
         bool isSeaLevelRef = false;

src/main/fc/fc_core.c

@@ -457,7 +457,6 @@ void disarm(disarmReason_t disarmReason)
 #ifdef USE_PROGRAMMING_FRAMEWORK
         programmingPidReset();
 #endif
-
         beeper(BEEPER_DISARMING);      // emit disarm tone
 
         prearmWasReset = false;

src/main/fc/fc_msp.c

@@ -2668,6 +2668,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
     case MSP_SET_WP:
         if (dataSize == 21) {
+            static uint8_t mmIdx = 0;
             const uint8_t msp_wp_no = sbufReadU8(src);     // get the waypoint number
             navWaypoint_t msp_wp;
             msp_wp.action = sbufReadU8(src);    // action
@@ -2679,6 +2680,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             msp_wp.p3 = sbufReadU16(src);       // P3
             msp_wp.flag = sbufReadU8(src);      // future: to set nav flag
             setWaypoint(msp_wp_no, &msp_wp);
+
+            uint8_t fwAppraochStartIdx = 8;
+#ifdef USE_SAFE_HOME
+            fwAppraochStartIdx = MAX_SAFE_HOMES;
+#endif
+            if (msp_wp_no == 0) {
+                mmIdx = 0;
+            } else if (msp_wp.flag == NAV_WP_FLAG_LAST) {
+                mmIdx++;
+            }
+            resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
         } else
             return MSP_RESULT_ERROR;
         break;
@@ -3137,6 +3149,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
             safeHomeConfigMutable(i)->lat = sbufReadU32(src);
             safeHomeConfigMutable(i)->lon = sbufReadU32(src);
+            resetFwAutolandApproach(i);
         } else {
             return MSP_RESULT_ERROR;
         }

src/main/fc/settings.yaml

@@ -2316,7 +2316,7 @@ groups:
         field: w_z_surface_p
         min: 0
         max: 100
-        default_value: 3.5
+        default_value: 3.5 
       - name: inav_w_z_surface_v
         field: w_z_surface_v
         min: 0
@@ -4090,8 +4090,8 @@ groups:
     condition: USE_SAFE_HOME
     members: 
       - name: nav_fw_land_approach_length
-        description: "Angle of the final approach"
+        description: "Length of the final approach"
-        default_value: 50000
+        default_value: 35000
         field: approachLength
         min: 100
         max: 100000
@@ -4103,13 +4103,13 @@ groups:
         max: 400
       - name: nav_fw_land_glide_alt
         description: "Initial altitude of the glide phase"
-        default_value: 500
+        default_value: 200
         field: glideAltitude
         min: 100
         max: 5000
       - name: nav_fw_land_flare_alt
         description: "Initial altitude of the flare phase"
-        default_value: 2000
+        default_value: 150
         field: flareAltitude
         min: 0
         max: 10000
@@ -4121,7 +4121,7 @@ groups:
         max: 45
       - name: nav_fw_land_flare_pitch
         description: "Pitch value for flare phase. In degrees"
-        default_value: 2
+        default_value: 8
         field: flarePitch
         min: 0
         max: 45

src/main/io/osd.c

@@ -1007,7 +1007,7 @@ static const char * osdFailsafeInfoMessage(void)
 #if defined(USE_SAFE_HOME)
 static const char * divertingToSafehomeMessage(void)
 {
-	if ((NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) || isFwLandInProgess()) {
+	if (!posControl.fwLandWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied)) {
 	    return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
 	}
 	return NULL;
@@ -5135,7 +5135,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
 
         if (ARMING_FLAG(ARMED)) {
             if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
-                if (isWaypointMissionRTHActive()) {
+                if (isWaypointMissionRTHActive() && !posControl.fwLandWp) {
                     // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
                     messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
                 }

src/main/navigation/navigation.c

@@ -66,6 +66,7 @@
 #include "sensors/gyro.h"
 
 #include "programming/global_variables.h"
+#include "sensors/rangefinder.h"
 
 // Multirotors:
 #define MR_RTH_CLIMB_OVERSHOOT_CM   100  // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
@@ -98,6 +99,7 @@ PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
     .approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
     .finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT,
     .flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT,
+    .glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT,
     .flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
     .maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
     .glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
@@ -847,6 +849,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
             [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD]          = NAV_STATE_COURSE_HOLD_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_CRUISE]               = NAV_STATE_CRUISE_INITIALIZE,
             [NAV_FSM_EVENT_SWITCH_TO_MIXERAT]              = NAV_STATE_MIXERAT_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED]    = NAV_STATE_WAYPOINT_FINISHED,
         }
     },
 
@@ -1136,10 +1140,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
-            [NAV_FSM_EVENT_TIMEOUT]         = NAV_STATE_FW_LANDING_ABORT,
+            [NAV_FSM_EVENT_TIMEOUT]             = NAV_STATE_FW_LANDING_ABORT,
-            [NAV_FSM_EVENT_SUCCESS]         = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SUCCESS]             = NAV_STATE_IDLE,
-            [NAV_FSM_EVENT_SWITCH_TO_RTH]   = NAV_STATE_RTH_INITIALIZE,
+            [NAV_FSM_EVENT_SWITCH_TO_RTH]       = NAV_STATE_RTH_INITIALIZE,
-            [NAV_FSM_EVENT_SWITCH_TO_IDLE]  = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]      = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT]  = NAV_STATE_WAYPOINT_INITIALIZE,
         }
     }, 
 };
@@ -1673,7 +1678,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
 
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
 {
-    UNUSED(previousState);
 
     //On ROVER and BOAT we immediately switch to the next event
     if (!STATE(ALTITUDE_CONTROL)) {
@@ -1695,13 +1699,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
     }
 
     if (STATE(FIXED_WING_LEGACY)) {
-        // FW Autoland configuured? 
+        // FW Autoland configured?
-        if (!posControl.fwLandAborted && safehome_index >= 0 && (fwAutolandApproachConfig(safehome_index)->landApproachHeading1 != 0 || fwAutolandApproachConfig(safehome_index)->landApproachHeading2 != 0)) {
+        int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8; 
-            posControl.fwLandPos = posControl.rthState.homePosition.pos;
+#ifdef USE_MULTI_MISSION
-            posControl.fwApproachSettingIdx = safehome_index;
+        missionIdx = posControl.loadedMultiMissionIndex - 1;
+#endif  
+
+#ifdef USE_SAFE_HOME
+        shIdx = safehome_index;
+        missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
+#endif
+        if (!posControl.fwLandAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) {
+            
+            if (previousState == NAV_STATE_WAYPOINT_REACHED) {
+                posControl.fwLandPos = posControl.activeWaypoint.pos;
+                posControl.fwApproachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
+                posControl.fwLandWp = true;
+            } else {
+                posControl.fwLandPos = posControl.rthState.homePosition.pos;
+                posControl.fwApproachSettingIdx = shIdx;
+                posControl.fwLandWp = false;
+            }
+            
             posControl.fwLandAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt;
             posControl.fwLandAproachAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt;
-            
             return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
         } else {
             return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@@ -1986,7 +2007,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
     UNUSED(previousState);
 
     const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
-    if (landEvent == NAV_FSM_EVENT_SUCCESS) {
+
+    if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
+        return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
+    } else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
+        return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
+    } else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
         // Landing controller returned success - invoke RTH finishing state and finish the waypoint
         navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
         return NAV_FSM_EVENT_SUCCESS;
@@ -2029,6 +2055,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
 {
     UNUSED(previousState);
 
+    posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
         resetPositionController();
         setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
@@ -2283,7 +2310,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
                 posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
                 
                 calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
-                tmpPos.z = posControl.fwLandAltAgl;
+                tmpPos.z = posControl.fwLandAproachAltAgl;
                 posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos;
 
                 setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]);
@@ -2319,7 +2346,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
     }
 
-    if (getEstimatedActualPosition(Z) <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
+    if (isLandingDetected()) {
+        posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
+        disarm(DISARM_LANDING);
+        return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+    }
+
+    float altitude;
+#ifdef USE_RANGEFINDER
+    if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
+        altitude = rangefinderGetLatestAltitude();
+    }
+    else
+#endif
+    if (posControl.flags.estAglStatus >= EST_USABLE) {
+        altitude = posControl.actualState.agl.pos.z;
+    } else {
+        altitude = posControl.actualState.abs.pos.z;
+    }
+
+    if (altitude <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
         resetPositionController();
         posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
         return NAV_FSM_EVENT_SUCCESS;
@@ -2356,9 +2402,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
         return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
     }
 
+    float altitude = -1;
+#ifdef USE_RANGEFINDER
+    if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
+        altitude = rangefinderGetLatestAltitude();
+    }
+#endif
     // Surface sensor present?
-    if (posControl.flags.estAglStatus == EST_TRUSTED) {
+    if (altitude >= 0) {
-        if (getEstimatedActualPosition(Z) <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
+        if (altitude <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
             posControl.cruise.course = posControl.fwLandingDirection;
             posControl.cruise.previousCourse = posControl.cruise.course;
             posControl.cruise.lastCourseAdjustmentTime = 0;
@@ -2366,6 +2418,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
             return NAV_FSM_EVENT_SUCCESS;
         }
     } else if (isLandingDetected()) {
+        posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
         return NAV_FSM_EVENT_SWITCH_TO_IDLE;
     }
     
@@ -2389,6 +2442,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
     UNUSED(previousState);
     
     if (isLandingDetected()) {
+        posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
         return NAV_FSM_EVENT_SUCCESS;
     }
     setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
@@ -2400,7 +2454,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
 {
     UNUSED(previousState);
     posControl.fwLandAborted = true;
-    return NAV_FSM_EVENT_SWITCH_TO_RTH;
+    posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
+
+    return posControl.fwLandWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
 }
 
 static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
@@ -4134,7 +4190,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
     posControl.flags.verticalPositionDataConsumed = false;
 
     if (!isFwLandInProgess()) {
-        posControl.fwLandState = FW_AUTOLAND_LC_STATE_IDLE;
+        posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
     }
 
     /* Process controllers */
@@ -5001,6 +5057,8 @@ static void resetFwAutoland(void)
     posControl.fwLandWpReached = false;
     posControl.fwApproachSettingIdx = 0;
     posControl.fwLandPosHeading = -1;
+    posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
+    posControl.fwLandWp = false;
 }
 
 static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
@@ -5043,9 +5101,14 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos
     posControl.wpAltitudeReached = false;
 }
 
-void resetFwAutolandApproach(void)
+void resetFwAutolandApproach(int8_t idx)
 {
-    memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
+    if (idx >= 0 && idx < MAX_FW_LAND_APPOACH_SETTINGS)
+    {
+        memset(fwAutolandApproachConfigMutable(idx), 0, sizeof(navFwAutolandApproach_t));
+    } else {
+        memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
+    }
 }
 
 bool isFwLandInProgess(void) 

src/main/navigation/navigation.h

@@ -59,7 +59,7 @@ typedef enum  {
 } fwAutolandApproachDirection_e;
 
 typedef enum {
-    FW_AUTOLAND_LC_STATE_IDLE,
+    FW_AUTOLAND_STATE_IDLE,
     FW_AUTOLAND_STATE_CROSSWIND,
     FW_AUTOLAND_STATE_BASE_LEG,
     FW_AUTOLAND_STATE_FINAL_APPROACH,
@@ -103,7 +103,7 @@ extern int8_t safehome_index;                    // -1 if no safehome, 0 to MAX_
 extern uint32_t safehome_distance;               // distance to the nearest safehome
 extern bool safehome_applied;                    // whether the safehome has been applied to home.
 
-void resetFwAutolandApproach(void);
+void resetFwAutolandApproach(int8_t idx);
 void resetSafeHomes(void);                       // remove all safehomes
 bool findNearestSafeHome(void);                  // Find nearest safehome
 

src/main/navigation/navigation_private.h

@@ -467,6 +467,7 @@ typedef struct {
     bool fwLandWpReached;
     fwAutolandWayppoints_t fwLandCurrentWp;
     bool fwLandAborted;
+    bool fwLandWp;
     fwAutolandState_t fwLandState;
 
     /* Internals & statistics */