From 57ae37cc389d2790e3f5e7aabad6699d6c0ba3a6 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Sep 2024 12:12:44 +0100 Subject: [PATCH] CR131 --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 6 ++--- src/main/navigation/navigation_fixedwing.c | 4 ++-- .../navigation/navigation_pos_estimator.c | 24 ++++++++++++------- .../navigation_pos_estimator_private.h | 2 ++ 5 files changed, 24 insertions(+), 14 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c7a60df1367..091fba50e68 100644 --- a/docs/Settings.md +++ b/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 | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d3b892fa43b..8d318a113f1 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 233685dfc38..1d807a00e75 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/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); diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 077a9784663..9a8ad2b101d 100644 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/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; } diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 1bfbda50a70..b9d059d9e51 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; // CR131 typedef struct {