diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c index 491c0f1c44..9e80cb0d15 100644 --- a/src/main/flight/gps_rescue.c +++ b/src/main/flight/gps_rescue.c @@ -303,6 +303,11 @@ void idleTasks() return; } + // Don't update any rescue flight statistics if we haven't applied a proper altitude offset yet + if (!isAltitudeOffset()) { + return; + } + gpsRescueAngle[AI_PITCH] = 0; gpsRescueAngle[AI_ROLL] = 0; diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 8d510e7f15..d55803d62a 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -44,12 +44,13 @@ static int32_t estimatedAltitude = 0; // in cm #if defined(USE_BARO) || defined(USE_GPS) +static bool altitudeOffsetSet = false; + void calculateEstimatedAltitude(timeUs_t currentTimeUs) { static timeUs_t previousTimeUs = 0; static int32_t baroAltOffset = 0; static int32_t gpsAltOffset = 0; - static bool altitudeOffsetSet = false; const uint32_t dTime = currentTimeUs - previousTimeUs; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) { @@ -111,6 +112,10 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { } #endif +bool isAltitudeOffset(void) +{ + return altitudeOffsetSet; +} int32_t getEstimatedAltitude(void) { diff --git a/src/main/flight/position.h b/src/main/flight/position.h index af7a3b3eca..c7b98173b4 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -22,6 +22,7 @@ #include "common/time.h" +bool isAltitudeOffset(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); int32_t getEstimatedAltitude(void); int16_t getEstimatedVario(void); \ No newline at end of file