diff --git a/docs/Settings.md b/docs/Settings.md index c2b3247ff9..817a6e2da2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1994,7 +1994,7 @@ Uncertainty value for barometric sensor [cm] ### inav_default_alt_sensor -Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. +Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a1f1ef7ff7..486fc54b8c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -199,7 +199,7 @@ tables: values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e - name: default_altitude_source - values: ["GPS", "BARO"] + values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] enum: navDefaultAltitudeSensor_e constants: @@ -2471,7 +2471,7 @@ groups: min: 0 max: 9999 - name: inav_default_alt_sensor - description: "Sets the default altitude sensor to use, GPS or BARO, when the altitude error between the sensors exceeds a set limit. Only the default sensor will be used in this case. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv." + description: "Sets the default altitude sensor to use. Settings GPS and BARO always use both sensors unless there is an altitude error between the sensors that exceeds a set limit. In this case only the selected sensor will be used while the altitude error limit is exceeded. GPS error limit = 2 * inav_max_eph_epv. BARO error limit = 4 * inav_baro_epv. Settings GPS_ONLY and BARO_ONLY will use only the selected sensor even if the other sensor is working. The other sensor will only be used as a backup if the selected sensor is no longer available to use." default_value: "GPS" field: default_alt_sensor table: default_altitude_source diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5bcfc1e11e..8332d8a70a 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -559,11 +559,11 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; - float wBaro = 1.0f; - float wGps = 1.0f; + const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + float wGps = defaultAltitudeSource == ALTITUDE_SOURCE_BARO_ONLY && ctx->newFlags & EST_BARO_VALID ? 0.0f : 1.0f; + float wBaro = defaultAltitudeSource == ALTITUDE_SOURCE_GPS_ONLY && ctx->newFlags & EST_GPS_Z_VALID ? 0.0f : 1.0f; - if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) { - const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor; + if (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) { const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); // Fade out the non default sensor to prevent sudden jump diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 46fcb42e17..5cbdc81c03 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -149,6 +149,8 @@ typedef enum { typedef enum { ALTITUDE_SOURCE_GPS, ALTITUDE_SOURCE_BARO, + ALTITUDE_SOURCE_GPS_ONLY, + ALTITUDE_SOURCE_BARO_ONLY, } navDefaultAltitudeSensor_e; typedef struct {