FW-Autoland

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 |
 | --- | --- |

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
 

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; 

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;
 

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];