diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 4854025a7f..3d9152d09f 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -78,20 +78,19 @@ void posHoldUpdateTargetLocation(void) } } -void posHoldNewGpsData(void) { - if (posHold.posHoldIsOK) { - posHoldUpdateTargetLocation(); - posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband); - } else { - autopilotAngle[AI_PITCH] = 0.0f; - autopilotAngle[AI_ROLL] = 0.0f; - } -} - void updatePosHold(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); // check for enabling Alt Hold, otherwise do as little as possible while inactive posHoldStart(); + if (posHold.posHoldIsOK) { + posHoldUpdateTargetLocation(); + if (isNewDataForPosHold() && posHold.posHoldIsOK) { + posHold.posHoldIsOK = positionControl(posHold.useStickAdjustment, posHold.deadband); + } + } else { + autopilotAngle[AI_PITCH] = 0.0f; + autopilotAngle[AI_ROLL] = 0.0f; + } } bool posHoldFailure(void) { diff --git a/src/main/io/gps.c b/src/main/io/gps.c index e5e182a903..7c8188e08f 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -95,6 +95,10 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N]; static serialPort_t *gpsPort; static float gpsDataIntervalSeconds; +#ifdef USE_POS_HOLD_MODE +static bool newDataForPosHold = false; +#endif + typedef struct gpsInitData_s { uint8_t index; uint8_t baudrateIndex; // see baudRate_e @@ -2596,7 +2600,7 @@ 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 + newDataForPosHold = true; #endif if (!STATE(GPS_FIX)) { @@ -2618,6 +2622,14 @@ void onGpsNewData(void) #endif // USE_GPS_LAP_TIMER } +#ifdef USE_POS_HOLD_MODE +bool isNewDataForPosHold(void) { + bool currentState = newDataForPosHold; // true only when new data arrives + newDataForPosHold = false; // clear flag once new data has been handled + return currentState; +} +#endif + void gpsSetFixState(bool state) { if (state) { diff --git a/src/main/io/gps.h b/src/main/io/gps.h index f28abc5259..fb6c649506 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -392,5 +392,6 @@ void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, void gpsSetFixState(bool state); float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue +bool isNewDataForPosHold(void); baudRate_e getGpsPortActualBaudRateIndex(void);