diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 912c2c88c5..dd2bb010db 100644 --- a/src/main/flight/autopilot.c +++ b/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; -} \ No newline at end of file +} diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 674b10d3c5..35e76c2800 100644 --- a/src/main/flight/pos_hold.c +++ b/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 diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index 6e24d3d779..60e0722f95 100644 --- a/src/main/flight/pos_hold.h +++ b/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 diff --git a/src/test/unit/althold_unittest.cc b/src/test/unit/althold_unittest.cc index 9b1698bf5e..4934809be8 100644 --- a/src/test/unit/althold_unittest.cc +++ b/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);