baro altitude estimation fix

breadoven · May 15, 2024 · 9c64e9b · 9c64e9b
1 parent b96c3d4
commit 9c64e9b
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 31 deletions.
diff --git a/docs/Settings.md b/docs/Settings.md
@@ -1812,16 +1812,6 @@ Allows to chose when the home position is reset. Can help prevent resetting home
 
 ---
 
-### inav_use_gps_no_baro
-
-Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed.
-
-| Default | Min | Max |
-| --- | --- | --- |
-| ON | OFF | ON |
-
----
-
 ### inav_w_acc_bias
 
 Weight for accelerometer drift estimation

diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
@@ -215,7 +215,7 @@ void initActiveBoxIds(void)
 
     bool navReadyAltControl = getHwBarometerStatus() != HW_SENSOR_NONE;
 #ifdef USE_GPS
-    navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro));
+    navReadyAltControl = navReadyAltControl || feature(FEATURE_GPS);
 
     const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning;
     bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS);

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
@@ -2271,11 +2271,6 @@ groups:
         field: gravity_calibration_tolerance
         min: 0
         max: 255
-      - name: inav_use_gps_no_baro
-        description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed."
-        field: use_gps_no_baro
-        type: bool
-        default_value: ON
       - name: inav_allow_dead_reckoning
         description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation"
         default_value: OFF

diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
@@ -260,8 +260,6 @@ typedef struct positionEstimationConfig_s {
     float max_eph_epv;  // Max estimated position error acceptable for estimation (cm)
     float baro_epv;     // Baro position error
 
-    uint8_t use_gps_no_baro;
-
 #ifdef USE_GPS_FIX_ESTIMATION
     uint8_t allow_gps_fix_estimation;
 #endif

diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
@@ -56,15 +56,14 @@
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 6);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
         .automatic_mag_declination = SETTING_INAV_AUTO_MAG_DECL_DEFAULT,
         .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT,
         .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
         .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT,  // 5 cm/s/s calibration error accepted (0.5% of gravity)
-        .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT,                      // Use GPS altitude if no baro is available on all aircrafts
         .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
 
         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
@@ -180,12 +179,12 @@ void onNewGPSData(void)
     newLLH.lon = gpsSol.llh.lon;
     newLLH.alt = gpsSol.llh.alt;
 
-    if (sensors(SENSOR_GPS) 
+    if (sensors(SENSOR_GPS)
 #ifdef USE_GPS_FIX_ESTIMATION
             || STATE(GPS_ESTIMATED_FIX)
 #endif
         ) {
-        if (!(STATE(GPS_FIX) 
+        if (!(STATE(GPS_FIX)
 #ifdef USE_GPS_FIX_ESTIMATION
                 || STATE(GPS_ESTIMATED_FIX)
 #endif
@@ -553,15 +552,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 
     bool correctOK = false;
-
-    //ignore baro if difference is too big, baro is probably wrong
-    const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
-    //fade out the baro to prevent sudden jump
-    const float start_epv = positionEstimationConfig()->max_eph_epv;
-    const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
-    const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
-    //use both baro and gps
-    if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
+
+    float wBaro = 0.0f;
+    if (ctx->newFlags & EST_BARO_VALID) {
+        // Ignore baro if difference is too big, baro is probably wrong
+        const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
+
+        // Fade out the baro to prevent sudden jump
+        const float start_epv = positionEstimationConfig()->max_eph_epv;
+        const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
+        wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
+    }
+
+    // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS
+    if (wBaro > 0.01f) {
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -597,6 +601,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
         correctOK = true;
     }
+
     if (ctx->newFlags & EST_GPS_Z_VALID) {
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {