diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 9bc20771af..e4b50c66df 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1714,6 +1714,7 @@ const clivalue_t valueTable[] = { // PG_POSITION { "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) }, + { "position_gps_alt_min_sats", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_POSITION, offsetof(positionConfig_t, gpsAltMinSats) }, // PG_MODE_ACTIVATION_CONFIG #if defined(USE_CUSTOM_BOX_NAMES) diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 2b783dadf7..5abc21bfad 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -56,6 +56,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1 PG_RESET_TEMPLATE(positionConfig_t, positionConfig, .altSource = DEFAULT, + .gpsAltMinSats = 9, ); static int32_t estimatedAltitudeCm = 0; // in cm @@ -87,6 +88,7 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) { #if defined(USE_BARO) || defined(USE_GPS) static bool altitudeOffsetSet = false; +static bool altitudeOffsetSetGPS = false; void calculateEstimatedAltitude(timeUs_t currentTimeUs) { @@ -103,6 +105,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) int32_t baroAlt = 0; int32_t gpsAlt = 0; + uint16_t gpsNumSat = 0; #if defined(USE_GPS) && defined(USE_VARIO) int16_t gpsVertSpeed = 0; @@ -124,6 +127,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) #ifdef USE_GPS if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { gpsAlt = gpsSol.llh.altCm; + gpsNumSat = gpsSol.numSat; #ifdef USE_VARIO gpsVertSpeed = GPS_verticalSpeedInCmS; #endif @@ -144,9 +148,23 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) { altitudeOffsetSet = false; } + baroAlt -= baroAltOffset; + + if (ARMING_FLAG(ARMED) && !altitudeOffsetSetGPS) { + if (haveBaroAlt && gpsNumSat >= positionConfig()->gpsAltMinSats && altitudeOffsetSet) { + gpsAltOffset = gpsAlt - baroAlt; + altitudeOffsetSetGPS = true; + } + } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) { + altitudeOffsetSetGPS = false; + } + gpsAlt -= gpsAltOffset; + if (!altitudeOffsetSetGPS) { + haveGpsAlt = false; + } if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) { if (ARMING_FLAG(ARMED)) { diff --git a/src/main/flight/position.h b/src/main/flight/position.h index ac528848aa..9bdcd20c28 100644 --- a/src/main/flight/position.h +++ b/src/main/flight/position.h @@ -24,6 +24,7 @@ typedef struct positionConfig_s { uint8_t altSource; + uint8_t gpsAltMinSats; } positionConfig_t; PG_DECLARE(positionConfig_t, positionConfig);