From ca0f25b265f9e97785ee61a0ceeb8d8925319860 Mon Sep 17 00:00:00 2001
From: Filipp Bakanov <filipp@bakanov.su>
Date: Fri, 28 Jan 2022 15:14:29 +0300
Subject: [PATCH] Refactor

---
 src/main/cli/settings.c    |  2 --
 src/main/flight/position.c | 31 ++++++++++++++++++++-----------
 src/main/flight/position.h |  1 -
 3 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 55e321aa76..c0515de0b8 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1718,8 +1718,6 @@ 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)
     { "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) },
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index 5abc21bfad..9dc88a5b93 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -55,8 +55,7 @@ typedef enum {
 PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 1);
 
 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
-    .altSource = DEFAULT,
-    .gpsAltMinSats = 9,
+.altSource = DEFAULT,
 );
 
 static int32_t estimatedAltitudeCm = 0;                // in cm
@@ -87,7 +86,7 @@ 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)
@@ -141,20 +140,29 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
     }
 #endif
 
-    if (ARMING_FLAG(ARMED) && !altitudeOffsetSet) {
+    if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
         baroAltOffset = baroAlt;
-        gpsAltOffset = gpsAlt;
-        altitudeOffsetSet = true;
-    } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
-        altitudeOffsetSet = false;
+        altitudeOffsetSetBaro = true;
+    } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
+        altitudeOffsetSetBaro = false;
     }
 
     baroAlt -= baroAltOffset;
 
-    if (ARMING_FLAG(ARMED) && !altitudeOffsetSetGPS) {
-        if (haveBaroAlt && gpsNumSat >= positionConfig()->gpsAltMinSats && altitudeOffsetSet) {
+    int goodGpsSats = 999;
+    int badGpsSats = 0;
+
+    if (haveBaroAlt) {
+        goodGpsSats = 10;
+        badGpsSats = 7;
+    }
+
+    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;
@@ -164,6 +172,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 
     if (!altitudeOffsetSetGPS) {
         haveGpsAlt = false;
+        gpsTrust = 0.0f;
     }
 
     if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
@@ -200,7 +209,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
 
 bool isAltitudeOffset(void)
 {
-    return altitudeOffsetSet;
+    return altitudeOffsetSetBaro || altitudeOffsetSetGPS;
 }
 #endif
 
diff --git a/src/main/flight/position.h b/src/main/flight/position.h
index 9bdcd20c28..ac528848aa 100644
--- a/src/main/flight/position.h
+++ b/src/main/flight/position.h
@@ -24,7 +24,6 @@
 
 typedef struct positionConfig_s {
     uint8_t altSource;
-    uint8_t gpsAltMinSats;
 } positionConfig_t;
 
 PG_DECLARE(positionConfig_t, positionConfig);