rearrange status checks

src/main/flight/autopilot.c

@@ -25,16 +25,13 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/vector.h"
-#include "fc/core.h"
 #include "fc/rc.h"
 #include "fc/runtime_config.h"
 
 #include "flight/imu.h"
-#include "flight/pos_hold.h"
 #include "flight/position.h"
 #include "rx/rx.h"
 #include "sensors/gyro.h"
-#include "sensors/compass.h"
 
 #include "autopilot.h"
 
@@ -203,25 +200,6 @@ void setTargetLocation(gpsLocation_t newTargetLocation) {
 
 bool positionControl(void) {
 
-    if (!STATE(GPS_FIX)) {
-        return false; // cannot proceed; this could happen mid-flight, e.g. early after takeoff without a fix
-    }
-
-    if (
-    #ifdef USE_MAG
-        !compassIsHealthy() && // if compass is OK, don't worry about GPS; this is not likely to change in-flight
-    #endif
-        (!allowPosHoldWithoutMag() || !canUseGPSHeading) // no compass, check if GPS heading is OK, which changes during the flight
-    ) {
-        return false;
-    }
-
-    if (!wasThrottleRaised()) {
-        return false;
-    }
-
-    // note: returning false dispays POS_HOLD_FAIL warning in OSD
-
     if (isNewGPSDataAvailable()) {
         posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
         posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
@@ -367,4 +345,4 @@ float getAutopilotThrottle(void)
 bool isAutopilotActive(void)
 {
     return !posHold.sticksActive;
-}
+}

src/main/flight/pos_hold.c

@@ -24,12 +24,15 @@
 #include "common/maths.h"
 
 #include "config/config.h"
+#include "fc/core.h"
 #include "fc/runtime_config.h"
 #include "fc/rc.h"
 #include "flight/autopilot.h"
 #include "flight/failsafe.h"
+#include "flight/imu.h"
 #include "flight/position.h"
 #include "rx/rx.h"
+#include "sensors/compass.h"
 
 #include "pos_hold.h"
 
@@ -67,7 +70,8 @@ void posHoldCheckSticks(void)
     }
 }
 
-void posHoldStart(void) {
+void posHoldStart(void)
+{
     static bool isInPosHoldMode = false;
     if (FLIGHT_MODE(POS_HOLD_MODE)) {
         if (!isInPosHoldMode) {
@@ -84,11 +88,28 @@ void posHoldStart(void) {
     }
 }
 
+bool posHoldStatusChecks(void)
+{
+    if (!STATE(GPS_FIX) || !wasThrottleRaised()) {
+        posHold.posHoldIsOK = false; // cannot continue, display POS_HOLD_FAIL warning in OSD
+        return false; 
+    }
+    if (
+#ifdef USE_MAG
+        !compassIsHealthy() &&
+#endif
+        (!posHoldConfig()->pos_hold_without_mag || !canUseGPSHeading)) {
+        posHold.posHoldIsOK = false;
+        return false; 
+    }
+    return true;
+}
+
 void updatePosHold(timeUs_t currentTimeUs) {
     UNUSED(currentTimeUs); 
     // check for enabling Pod Hold, otherwise do as little as possible while inactive
     posHoldStart();
-    if (posHold.posHoldIsOK) {
+    if (posHold.posHoldIsOK && posHoldStatusChecks()) {
         posHoldCheckSticks();
         posHold.posHoldIsOK = positionControl();
     } else {
@@ -101,8 +122,4 @@ bool posHoldFailure(void) {
     return (FLIGHT_MODE(POS_HOLD_MODE) && !posHold.posHoldIsOK);
 }
 
-bool allowPosHoldWithoutMag(void) {
-    return (posHoldConfig()->pos_hold_without_mag);
-}
-
 #endif // USE_POS_HOLD

src/main/flight/pos_hold.h

@@ -19,7 +19,7 @@
 
 #include "pg/pos_hold.h"
 
-#ifdef USE_ALT_HOLD_MODE
+#ifdef USE_POS_HOLD_MODE
 #include "common/time.h"
 #include "io/gps.h"
 
@@ -30,6 +30,5 @@ void posHoldNewGpsData(void);
 void updatePosHold(timeUs_t currentTimeUs);
 
 bool posHoldFailure(void);
-bool allowPosHoldWithoutMag(void);
 
 #endif

src/test/unit/althold_unittest.cc

@@ -127,13 +127,12 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
     gpsSolutionData_t gpsSol;
     bool canUseGPSHeading;
     bool compassIsHealthy;
+    bool wasThrottleRaised(void) { return true; }
     float getGpsDataIntervalSeconds(void) { return 0.01f; }
     float getRcDeflectionAbs(void) { return 0.0f; }
     attitudeEulerAngles_t attitude;
     bool isNewGPSDataAvailable(void){ return true; }
-    bool wasThrottleRaised(void) { return true; }
-
-
+    
     void parseRcChannels(const char *input, rxConfig_t *rxConfig)
     {
         UNUSED(input);