From 8641fcd2918e6c586c38a1bfc471a21e097dbee8 Mon Sep 17 00:00:00 2001
From: ctzsnooze <chris@th.id.au>
Date: Sat, 19 Oct 2024 10:45:24 +1100
Subject: [PATCH] fix failure to initiate position hold from error in ifdef

---
 src/main/flight/autopilot.c |  5 +++--
 src/main/flight/pos_hold.c  | 10 +++++-----
 src/main/flight/pos_hold.h  |  1 +
 src/main/io/gps.c           | 13 +++++++------
 4 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c
index 1a36ace7d3..9b6e93fef6 100644
--- a/src/main/flight/autopilot.c
+++ b/src/main/flight/autopilot.c
@@ -201,10 +201,11 @@ bool positionControl(bool useStickAdjustment, float deadband) {
             posHold.previousDistanceCm = posHold.distanceCm;
         }
     }
+
     const uint8_t startLogger = posHold.justStarted ? 2 : 1;
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, startLogger);
-    
-    // simple (very simple) sanity check
+
+    // ** simple (too simple) sanity check **
     // primarily to detect flyaway from no Mag or badly oriented Mag
     // TODO - maybe figure how to make a better check by giving more leeway at the start?
     if (posHold.distanceCm > 1000) {
diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c
index 4dd0670363..e46aa29c3d 100644
--- a/src/main/flight/pos_hold.c
+++ b/src/main/flight/pos_hold.c
@@ -37,7 +37,7 @@ static posHoldState_t posHold;
 
 void posHoldReset(void)
 {
-    posHold.posHoldIsOK = true;
+    posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
     posHold.targetLocation = gpsSol.llh;
     resetPositionControl(posHold.targetLocation);
 }
@@ -46,6 +46,7 @@ void posHoldInit(void)
 {
     posHold.deadband = rcControlsConfig()->pos_hold_deadband / 100.0f;
     posHold.useStickAdjustment = rcControlsConfig()->pos_hold_deadband;
+    posHold.posHoldIsOK = false;
 }
 
 void posHoldStart(void) {
@@ -56,6 +57,7 @@ void posHoldStart(void) {
             wasInPosHoldMode = true;
         }
     } else {
+        posHold.posHoldIsOK = false;
         wasInPosHoldMode = false;
     }
 }
@@ -77,11 +79,9 @@ void posHoldUpdateTargetLocation(void)
 }
 
 void posHoldNewGpsData(void) {
-    if (FLIGHT_MODE(POS_HOLD_MODE)) {
+    if (posHold.posHoldIsOK) {
         posHoldUpdateTargetLocation();
-        if (posHold.posHoldIsOK) {
-            posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband);
-        }
+        posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband);
     } else {
         autopilotAngle[AI_PITCH] = 0.0f;
         autopilotAngle[AI_ROLL] = 0.0f;
diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h
index a34d48e90c..d3c3f75101 100644
--- a/src/main/flight/pos_hold.h
+++ b/src/main/flight/pos_hold.h
@@ -33,6 +33,7 @@ typedef struct {
 } posHoldState_t;
 
 void posHoldInit(void);
+void posHoldNewGpsData(void);
 void updatePosHold(timeUs_t currentTimeUs);
 
 bool posHoldFailure(void);
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index f62a815fd8..e5e182a903 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -56,6 +56,7 @@
 #include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
+#include "flight/pos_hold.h"
 
 #include "scheduler/scheduler.h"
 
@@ -2592,13 +2593,17 @@ void GPS_calculateDistanceAndDirectionToHome(void)
 
 void onGpsNewData(void)
 {
+    gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
+
+#ifdef USE_POS_HOLD_MODE
+    posHoldNewGpsData(); // pos hold code itself checks for GPS fix and shows OSD warning if not available
+#endif
+
     if (!STATE(GPS_FIX)) {
         // if we don't have a 3D fix don't give data to GPS rescue
         return;
     }
 
-    gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
-
     GPS_calculateDistanceAndDirectionToHome();
     if (ARMING_FLAG(ARMED)) {
         GPS_calculateDistanceFlown(false);
@@ -2611,10 +2616,6 @@ void onGpsNewData(void)
 #ifdef USE_GPS_LAP_TIMER
     gpsLapTimerNewGpsData();
 #endif // USE_GPS_LAP_TIMER
-
-#ifdef USE_POS_HOLD
-    posHoldNewGpsData();
-#endif
 }
 
 void gpsSetFixState(bool state)