From f5e209cbd787a4c07d627c2eb564d5bbff60e2a8 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 17 Jul 2024 09:01:27 +0100 Subject: [PATCH 1/5] nav_vel_z_improvement --- .../navigation/navigation_pos_estimator.c | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d5a342173d3..dba1209b078 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -342,6 +342,12 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } + +static bool gravityCalibrationIsValid(void) +{ + return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS; +} + #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -409,12 +415,13 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) { - if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { + if (!ARMING_FLAG(WAS_EVER_ARMED)) { zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); if (gravityCalibrationComplete()) { zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); setGravityCalibration(posEstimator.imu.calibratedGravityCMSS); + restartGravityCalibration(); // Repeat calibration until first arm to continuously update zero datum LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); } } @@ -423,8 +430,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - /* If calibration is incomplete - report zero acceleration */ - if (gravityCalibrationComplete()) { + if (gravityCalibrationIsValid()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS; @@ -432,7 +438,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) #endif posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS; } - else { + else { // If calibration is incomplete - report zero acceleration posEstimator.imu.accelNEU.x = 0.0f; posEstimator.imu.accelNEU.y = 0.0f; posEstimator.imu.accelNEU.z = 0.0f; @@ -521,7 +527,9 @@ static void estimationPredict(estimationContext_t * ctx) if ((ctx->newFlags & EST_Z_VALID)) { posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt; posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f; - posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + if (ARMING_FLAG(WAS_EVER_ARMED)) { // Hold at zero until first armed + posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt; + } } /* Prediction step: XY-axis */ @@ -599,7 +607,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } if (ctx->newFlags & EST_GPS_Z_VALID) { @@ -623,7 +631,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } return correctOK; @@ -907,5 +915,5 @@ void updatePositionEstimator(void) bool navIsCalibrationComplete(void) { - return gravityCalibrationComplete(); + return gravityCalibrationIsValid(); } From 569bd14515a97e55186fde9f982a9d9d1189dddb Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 23 Jul 2024 10:03:13 +0100 Subject: [PATCH 2/5] add default altitude source setting --- docs/Settings.md | 10 ++++ src/main/fc/settings.yaml | 10 +++- src/main/navigation/navigation.h | 29 +++++----- .../navigation/navigation_pos_estimator.c | 58 ++++++++++++------- .../navigation_pos_estimator_private.h | 5 ++ 5 files changed, 75 insertions(+), 37 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 6487b44b6d1..9afdd789685 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1992,6 +1992,16 @@ 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. + +| Default | Min | Max | +| --- | --- | --- | +| GPS | | | + +--- + ### inav_gravity_cal_tolerance Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 00b08ca698c..c65d0ffa768 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -83,7 +83,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"] - name: aux_operator values: ["OR", "AND"] @@ -198,6 +198,9 @@ tables: - name: headtracker_dev_type values: ["NONE", "SERIAL", "MSP"] enum: headTrackerDevType_e + - name: default_altitude_source + values: ["GPS", "BARO"] + enum: navDefaultAltitudeSensor_e constants: RPYL_PID_MIN: 0 @@ -2461,6 +2464,11 @@ groups: field: baro_epv 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." + default_value: "GPS" + field: default_alt_sensor + table: default_altitude_source - name: PG_NAV_CONFIG type: navConfig_t diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..da65f0f3f57 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -231,35 +231,36 @@ typedef enum { typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; - uint8_t reset_altitude_type; // from nav_reset_type_e - uint8_t reset_home_type; // nav_reset_type_e - uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) + uint8_t reset_altitude_type; // from nav_reset_type_e + uint8_t reset_home_type; // nav_reset_type_e + uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) uint8_t allow_dead_reckoning; uint16_t max_surface_altitude; - float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements - float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements - float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements + float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements + float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements - float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes - float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements + float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes + float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements - float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements - float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements + float w_xy_gps_p; // Weight (cutoff frequency) for GPS position measurements + float w_xy_gps_v; // Weight (cutoff frequency) for GPS velocity measurements float w_xy_flow_p; float w_xy_flow_v; - float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight + float w_z_res_v; // When velocity sources lost slowly decrease estimated velocity with this weight float w_xy_res_v; - float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. + float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable. - float max_eph_epv; // Max estimated position error acceptable for estimation (cm) - float baro_epv; // Baro position error + float max_eph_epv; // Max estimated position error acceptable for estimation (cm) + float baro_epv; // Baro position error + uint8_t default_alt_sensor; // default altitude sensor source #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 index dba1209b078..e11342ff38c 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -56,7 +56,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -89,6 +89,8 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT, + + .default_alt_sensor = SETTING_INAV_DEFAULT_ALT_SENSOR_DEFAULT, #ifdef USE_GPS_FIX_ESTIMATION .allow_gps_fix_estimation = SETTING_INAV_ALLOW_GPS_FIX_ESTIMATION_DEFAULT #endif @@ -560,20 +562,28 @@ 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; + + 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); - 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 non default sensor to prevent sudden jump + uint16_t residualErrorEpvLimit = defaultAltitudeSource == ALTITUDE_SOURCE_BARO ? 2 * positionEstimationConfig()->baro_epv : positionEstimationConfig()->max_eph_epv; + const float start_epv = residualErrorEpvLimit; + const float end_epv = residualErrorEpvLimit * 2.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; + // Calculate residual gps/baro sensor weighting based on assumed default altitude source = GPS wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); + + if (defaultAltitudeSource == ALTITUDE_SOURCE_BARO) { // flip residual sensor weighting if default = BARO + wGps = wBaro; + wBaro = 1.0f; + } } - // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS - if (wBaro > 0.01f) { + if (ctx->newFlags & EST_BARO_VALID && wBaro) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -596,21 +606,24 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); // Altitude - const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z; - ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + 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); + + ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; + ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } - if (ctx->newFlags & EST_GPS_Z_VALID) { + if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { // 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; @@ -619,16 +632,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } else { // Altitude - const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z; - const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z; + const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); + const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + + ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; + ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; - ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 44f0333d149..46fcb42e177 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -146,6 +146,11 @@ typedef enum { EST_Z_VALID = (1 << 6), } navPositionEstimationFlags_e; +typedef enum { + ALTITUDE_SOURCE_GPS, + ALTITUDE_SOURCE_BARO, +} navDefaultAltitudeSensor_e; + typedef struct { timeUs_t baroGroundTimeout; float baroGroundAlt; From cc5917558daaa3ba2544105b195dd81c8d4b753a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Aug 2024 13:41:28 +0100 Subject: [PATCH 3/5] remove gravity calibration changes --- src/main/navigation/navigation_pos_estimator.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index e11342ff38c..84c919c8a8b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -345,11 +345,6 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } -static bool gravityCalibrationIsValid(void) -{ - return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS; -} - #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -417,13 +412,12 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) { - if (!ARMING_FLAG(WAS_EVER_ARMED)) { + if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) { zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z); if (gravityCalibrationComplete()) { zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS); setGravityCalibration(posEstimator.imu.calibratedGravityCMSS); - restartGravityCalibration(); // Repeat calibration until first arm to continuously update zero datum LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS)); } } @@ -432,7 +426,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - if (gravityCalibrationIsValid()) { + if (gravityCalibrationComplete()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS; @@ -929,5 +923,5 @@ void updatePositionEstimator(void) bool navIsCalibrationComplete(void) { - return gravityCalibrationIsValid(); + return gravityCalibrationComplete(); } From ef2d9c1084bd7883ea9522aafb63bf9648248c7c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 9 Sep 2024 13:54:08 +0100 Subject: [PATCH 4/5] add baro v setting, clean up config calls --- docs/Settings.md | 10 ++++ src/main/fc/settings.yaml | 6 +++ src/main/navigation/navigation.h | 1 + .../navigation/navigation_pos_estimator.c | 54 +++++++++++-------- 4 files changed, 48 insertions(+), 23 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9afdd789685..c2b3247ff95 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2122,6 +2122,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i --- +### inav_w_z_baro_v + +Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. + +| Default | Min | Max | +| --- | --- | --- | +| 0.1 | 0 | 10 | + +--- + ### inav_w_z_gps_p Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 28279aff1e0..a1f1ef7ff74 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2410,6 +2410,12 @@ groups: min: 0 max: 10 default_value: 0.35 + - name: inav_w_z_baro_v + description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." + field: w_z_baro_v + min: 0 + max: 10 + default_value: 0.1 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index da65f0f3f57..24b8daa0402 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -239,6 +239,7 @@ typedef struct positionEstimationConfig_s { uint16_t max_surface_altitude; float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements + float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 84c919c8a8b..5bcfc1e11eb 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -69,6 +69,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, + .w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, @@ -478,14 +479,16 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) /* Figure out if we have valid position data from our data sources */ uint32_t newFlags = 0; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + if ((sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif ) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) && - (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) { - if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) { + (posEstimator.gps.eph < max_eph_epv)) { + if (posEstimator.gps.epv < max_eph_epv) { newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID; } else { @@ -505,11 +508,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs) newFlags |= EST_FLOW_VALID; } - if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.eph < max_eph_epv) { newFlags |= EST_XY_VALID; } - if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) { + if (posEstimator.est.epv < max_eph_epv) { newFlags |= EST_Z_VALID; } @@ -602,16 +605,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) // Altitude 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; - ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt; + 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; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -628,15 +632,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) // Altitude const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt; + ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; + ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), positionEstimationConfig()->w_z_gps_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResidual), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed @@ -716,21 +721,23 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) { estimationContext_t ctx; + const float max_eph_epv = positionEstimationConfig()->max_eph_epv; + /* Calculate dT */ ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime); posEstimator.est.lastUpdateTime = currentTimeUs; /* If IMU is not ready we can't estimate anything */ if (!isImuReady()) { - posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f; - posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f; + posEstimator.est.eph = max_eph_epv + 0.001f; + posEstimator.est.epv = max_eph_epv + 0.001f; posEstimator.flags = 0; return; } /* Calculate new EPH and EPV for the case we didn't update postion */ - ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); - ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); + ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); vectorZero(&ctx.estPosCorr); vectorZero(&ctx.estVelCorr); @@ -753,12 +760,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) estimationCalculateCorrection_XY_FLOW(&ctx); // If we can't apply correction or accuracy is off the charts - decay velocity to zero - if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) { + if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) { ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt; ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt; } - if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) { + if (!estZCorrectOk || ctx.newEPV > max_eph_epv) { ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; } // Boost the corrections based on accWeight @@ -770,16 +777,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); /* Correct accelerometer bias */ - if (positionEstimationConfig()->w_acc_bias > 0.0f) { + const float w_acc_bias = positionEstimationConfig()->w_acc_bias; + if (w_acc_bias > 0.0f) { const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { /* transform error vector from NEU frame to body frame */ imuTransformVectorEarthToBody(&ctx.accBiasCorr); /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; } } From d0b5dbee4ed7a832bde6d18bf54ff9d53e9f18ff Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 10 Sep 2024 09:41:22 +0100 Subject: [PATCH 5/5] add new settings --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_pos_estimator.c | 8 ++++---- src/main/navigation/navigation_pos_estimator_private.h | 2 ++ 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index c2b3247ff95..817a6e2da28 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 a1f1ef7ff74..486fc54b8c1 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 5bcfc1e11eb..8332d8a70aa 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 46fcb42e177..5cbdc81c030 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 {