Merge pull request #11312 from bakwc/fixDefaultModeAltitude

Fixed wrong altitude in DEFAULT (GPS + BARO) mode

src/main/cli/settings.c

@@ -1717,8 +1717,9 @@ const clivalue_t valueTable[] = {
 #endif
 
 // PG_POSITION
-    { "position_alt_source",           VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
+    { "position_alt_source",                    VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
-
+    { "position_alt_gps_min_sats",              VAR_INT8   | MASTER_VALUE, .config.minmaxUnsigned = { 4, 50 }, PG_POSITION, offsetof(positionConfig_t, altNumSatsGpsUse) },
+    { "position_alt_baro_fallback_sats",        VAR_INT8   | MASTER_VALUE, .config.minmaxUnsigned = { 4, 50 }, PG_POSITION, offsetof(positionConfig_t, altNumSatsBaroFallback) },
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)
     { "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) },

src/main/config/config.c

@@ -54,6 +54,7 @@
 #include "flight/pid_init.h"
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
+#include "flight/position.h"
 
 #include "io/beeper.h"
 #include "io/gps.h"
@@ -201,6 +202,14 @@ static void validateAndFixRatesSettings(void)
     }
 }
 
+static void validateAndFixPositionConfig(void)
+{
+    if (positionConfig()->altNumSatsBaroFallback >= positionConfig()->altNumSatsGpsUse) {
+        positionConfigMutable()->altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE;
+        positionConfigMutable()->altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK;
+    }
+}
+
 static void validateAndFixConfig(void)
 {
 #if !defined(USE_QUAD_MIXER_ONLY)
@@ -586,6 +595,8 @@ static void validateAndFixConfig(void)
     // This should be done at the end of the validation
     targetValidateConfiguration();
 #endif
+
+    validateAndFixPositionConfig();
 }
 
 void validateAndFixGyroConfig(void)

src/main/flight/position.c

@@ -52,10 +52,12 @@ typedef enum {
     GPS_ONLY
 } altSource_e;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 2);
 
 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
     .altSource = DEFAULT,
+    .altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE,
+    .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK,
 );
 
 static int32_t estimatedAltitudeCm = 0;                // in cm
@@ -86,7 +88,8 @@ int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
 #endif
 
 #if defined(USE_BARO) || defined(USE_GPS)
-static bool altitudeOffsetSet = false;
+static bool altitudeOffsetSetBaro = false;
+static bool altitudeOffsetSetGPS = false;
 
 void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 {
@@ -103,6 +106,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 
     int32_t baroAlt = 0;
     int32_t gpsAlt = 0;
+    uint8_t gpsNumSat = 0;
 
 #if defined(USE_GPS) && defined(USE_VARIO)
     int16_t gpsVertSpeed = 0;
@@ -124,6 +128,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
@@ -137,16 +142,40 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     }
 #endif
 
-    if (ARMING_FLAG(ARMED) && !altitudeOffsetSet) {
+    if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
         baroAltOffset = baroAlt;
-        gpsAltOffset = gpsAlt;
+        altitudeOffsetSetBaro = true;
-        altitudeOffsetSet = true;
+    } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
-    } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
+        altitudeOffsetSetBaro = false;
-        altitudeOffsetSet = false;
     }
+
     baroAlt -= baroAltOffset;
+
+    int goodGpsSats = 0;
+    int badGpsSats = -1;
+
+    if (haveBaroAlt) {
+        goodGpsSats = positionConfig()->altNumSatsGpsUse;
+        badGpsSats = positionConfig()->altNumSatsBaroFallback;
+    }
+
+    if (ARMING_FLAG(ARMED)) {
+        if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) {
+            gpsAltOffset = gpsAlt - baroAlt;
+            altitudeOffsetSetGPS = true;
+        } else if (gpsNumSat <= badGpsSats) {
+            altitudeOffsetSetGPS = false;
+        }
+    } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
+        altitudeOffsetSetGPS = false;
+    }
+
     gpsAlt -= gpsAltOffset;
 
+    if (!altitudeOffsetSetGPS) {
+        haveGpsAlt = false;
+        gpsTrust = 0.0f;
+    }
 
     if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
         if (ARMING_FLAG(ARMED)) {
@@ -182,7 +211,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 
 bool isAltitudeOffset(void)
 {
-    return altitudeOffsetSet;
+    return altitudeOffsetSetBaro || altitudeOffsetSetGPS;
 }
 #endif
 

src/main/flight/position.h

@@ -22,8 +22,13 @@
 
 #include "common/time.h"
 
+#define POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE 10
+#define POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK 7
+
 typedef struct positionConfig_s {
     uint8_t altSource;
+    uint8_t altNumSatsGpsUse;
+    uint8_t altNumSatsBaroFallback;
 } positionConfig_t;
 
 PG_DECLARE(positionConfig_t, positionConfig);