From 8e3ee38e518b51a70714e77fa6e8d1aa8e2cd269 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 22 Oct 2023 16:51:09 +0900 Subject: [PATCH] development of posestimator fusion --- src/main/fc/settings.yaml | 4 ++-- src/main/navigation/navigation_pos_estimator.c | 16 +++++++++------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 87daf753cf..c719c4003f 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2314,13 +2314,13 @@ groups: max: 100 default_value: 2.0 - name: inav_w_z_baro_p - description: "Weight of barometer measurements in estimated altitude and climb rate" + description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors." field: w_z_baro_p min: 0 max: 10 default_value: 0.35 - name: inav_w_z_gps_p - description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes" + description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p min: 0 max: 10 diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddd..d83836bfb1 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -570,7 +570,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count - if (ctx->newFlags & EST_BARO_VALID) { + bool correctOK = false; + //use both baro and gps + + if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro)) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -589,7 +592,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } } - // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it + // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); @@ -614,10 +617,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); } - return true; + correctOK = true; } - else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) { - // If baro is not available - use GPS Z for correction on a plane + if (ctx->newFlags & EST_GPS_Z_VALID) { // 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; @@ -637,10 +639,10 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); } - return true; + correctOK = true; } - return false; + return correctOK; } static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)