CR131

docs/Settings.md

@@ -1974,7 +1974,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 or BARO always uses both sensors with priority given to the selected sensor when the altitude error between the sensors exceeds a set limit. Only the selected sensor will be used in this case. 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 if the selected sensor is no longer available to use.
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/fc/settings.yaml

@@ -198,7 +198,7 @@ tables:
     values: ["NONE", "SERIAL", "MSP"]
     enum: headTrackerDevType_e
   - name: default_altitude_source  # CR131
-    values: ["GPS", "BARO"]
+    values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"]
     enum: navDefaultAltitudeSensor_e
 
 constants:
@@ -2418,7 +2418,7 @@ groups:
         field: w_z_baro_v
         min: 0
         max: 10
-        default_value: 0.1        
+        default_value: 0.1
         # CR131
       - name: inav_w_z_gps_p
         description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
@@ -2476,7 +2476,7 @@ groups:
         max: 9999
         # CR131
       - 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 with priority given to the selected sensor when the altitude error between the sensors exceeds a set limit. Only the selected sensor will be used in this case. 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

src/main/navigation/navigation_fixedwing.c

@@ -149,7 +149,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 
     // CR133
     // ****************** TEST FILTER VERT VEL
-    DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate);
+    // DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate);
     static pt1Filter_t velz2FilterState;
     currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, 0.1 * navConfig()->fw.wp_tracking_accuracy, US2S(deltaMicros));
     // DEBUG_SET(DEBUG_ALWAYS, 1, currentClimbRate);
@@ -487,7 +487,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
             }
 
             // DEBUG_SET(DEBUG_ALWAYS, 0, navCrossTrackError);
-            DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing);
+            // DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing);
             // DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate);
 
             uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);

src/main/navigation/navigation_pos_estimator.c

@@ -576,12 +576,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 
     bool correctOK = false;
 
-    float wBaro = 1.0f;
-    float wGps = 1.0f;  // CR131
+    // float wBaro = 1.0f;
+    // float wGps = 1.0f;  // CR131
+
+    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 (wBaro && ctx->newFlags & EST_BARO_VALID && wGps && ctx->newFlags & EST_GPS_Z_VALID) {
+    // if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) {
 
-    // CR131
-    if (ctx->newFlags & EST_BARO_VALID && ctx->newFlags & EST_GPS_Z_VALID) {
-        const uint8_t defaultAltitudeSource = positionEstimationConfig()->default_alt_sensor;
         const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt);
 
         // Fade out the non default sensor to prevent sudden jump  INAV_GPS_DEFAULT_EPV
@@ -596,7 +600,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         }
     }
     // CR131
+        DEBUG_SET(DEBUG_ALWAYS, 2, 10);
     if (ctx->newFlags & EST_BARO_VALID && wBaro) { // CR131
+        DEBUG_SET(DEBUG_ALWAYS, 2, 100);
         timeUs_t currentTimeUs = micros();
 
         if (!ARMING_FLAG(ARMED)) {
@@ -623,7 +629,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
         const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
         const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;  // CR131_X
-        // DEBUG_SET(DEBUG_ALWAYS, 2, baroVelZResidual);
+
         ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
         ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
         ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;  // CR131_X
@@ -638,8 +644,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);
         // CR131
     }
-
+DEBUG_SET(DEBUG_ALWAYS, 3, 10);
     if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {  // CR131
+        DEBUG_SET(DEBUG_ALWAYS, 3, 100);
         // Reset current estimate to GPS altitude if estimate not valid
         if (!(ctx->newFlags & EST_Z_VALID)) {
             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
@@ -666,7 +673,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         correctOK = ARMING_FLAG(WAS_EVER_ARMED);
         // CR131
     }
-
+DEBUG_SET(DEBUG_ALWAYS, 0, wBaro);
+DEBUG_SET(DEBUG_ALWAYS, 1, wGps);
     return correctOK;
 }
 

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;
 // CR131
 typedef struct {