diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md
index af2a731ffa..2bbcc29a53 100644
--- a/docs/Fixed Wing Landing.md	
+++ b/docs/Fixed Wing Landing.md	
@@ -83,7 +83,7 @@ If the altitude of the waypoint and the "Approach Altitude" are different, the a
 
 ## 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 current landing state can be retrieved via ID 41 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deplyoment of the landing flaps.
 
 | Returned value | State |
 | --- | --- |
diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md
index 943bb6ffd6..9f95fdb25d 100644
--- a/docs/Programming Framework.md	
+++ b/docs/Programming Framework.md	
@@ -156,7 +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 |
+| 41            | 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
 
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 91db8414f4..c248b67926 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -291,11 +291,6 @@ bool validateRTHSanityChecker(void);
 void updateHomePosition(void);
 bool abortLaunchAllowed(void);
 
-static bool rthAltControlStickOverrideCheck(unsigned axis);
-
-static void updateRthTrackback(bool forceSaveTrackPoint);
-static fpVector3_t * rthGetTrackbackPos(void);
-
 static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
 static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
 static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
@@ -4152,11 +4147,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
         //  ensure WP missions always restart from first waypoint after disarm
         posControl.activeWaypointIndex = posControl.startWpIndex;
         // Reset RTH trackback
-        posControl.activeRthTBPointIndex = -1;
-        posControl.flags.rthTrackbackActive = false;
-        posControl.rthTBWrapAroundCounter = -1;
-        posControl.fwLandState.landAborted = false;
-        posControl.fwLandState.approachSettingIdx = 0;
+        resetRthTrackBack();
         return;
     }
 
@@ -5024,6 +5015,29 @@ int32_t navigationGetHeadingError(void)
     return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
 }
 
+int8_t navCheckActiveAngleHoldAxis(void)
+{
+    int8_t activeAxis = -1;
+
+    if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
+        navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+        bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
+
+        if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
+            activeAxis = FD_PITCH;
+        } else if (altholdActive) {
+            activeAxis = FD_ROLL;
+        }
+    }
+
+    return activeAxis;
+}
+
+uint8_t getActiveWpNumber(void)
+{
+    return NAV_Status.activeWpNumber;
+}
+
 static void resetFwAutoland(void)
 {
     posControl.fwLandState.landAltAgl = 0; 
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 946a9e3d66..baccefcdd1 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -465,12 +465,6 @@ typedef struct {
     timeMs_t                    wpReachedTime;              // Time the waypoint was reached
     bool                        wpAltitudeReached;          // WP altitude achieved
 
-    /* RTH Trackback */
-    fpVector3_t                 rthTBPointsList[NAV_RTH_TRACKBACK_POINTS];
-    int8_t                      rthTBLastSavedIndex;        // last trackback point index saved
-    int8_t                      activeRthTBPointIndex;
-    int8_t                      rthTBWrapAroundCounter;     // stores trackpoint array overwrite index position
-
     /* Fixedwing autoland */
     fwLandState_t fwLandState;
 
diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c
index 7a303cd50b..85aea6489c 100644
--- a/src/main/target/SITL/sim/realFlight.c
+++ b/src/main/target/SITL/sim/realFlight.c
@@ -59,8 +59,8 @@
 #define RF_PORT 18083
 #define RF_MAX_CHANNEL_COUNT 12
 // RealFlight scenerys doesn't represent real landscapes, so fake some nice coords
-#define FAKE_LAT 0.0f
-#define FAKE_LON 0.0f
+#define FAKE_LAT 37.277127f
+#define FAKE_LON -115.799669f
 
 
 static uint8_t pwmMapping[RF_MAX_PWM_OUTS];