diff --git a/docs/Settings.md b/docs/Settings.md index a8fce544bff..ae0d3006ae6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2002,6 +2002,16 @@ Uncertainty value for barometric sensor [cm] --- +### inav_default_alt_sensor + +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 | +| --- | --- | --- | +| GPS | | | + +--- + ### inav_gravity_cal_tolerance Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. @@ -2122,6 +2132,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 03edb7af6a3..e43ad233b0c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -201,6 +201,9 @@ tables: - name: mavlink_radio_type values: ["GENERIC", "ELRS", "SIK"] enum: mavlinkRadio_e + - name: default_altitude_source + values: ["GPS", "BARO", "GPS ONLY", "BARO ONLY"] + enum: navDefaultAltitudeSensor_e constants: RPYL_PID_MIN: 0 @@ -2410,6 +2413,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 @@ -2464,6 +2473,11 @@ groups: field: baro_epv min: 0 max: 9999 + - name: inav_default_alt_sensor + 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 - name: PG_NAV_CONFIG type: navConfig_t diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1b7734c8b13..24b8daa0402 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -231,35 +231,37 @@ 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_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 + 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 d20087f9e32..e23b6e1f3cf 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 @@ -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, @@ -89,6 +90,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 @@ -342,6 +345,7 @@ static bool gravityCalibrationComplete(void) return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration); } + #define ACC_VIB_FACTOR_S 1.0f #define ACC_VIB_FACTOR_E 3.0f static void updateIMUEstimationWeight(const float dt) @@ -423,7 +427,6 @@ static void updateIMUTopic(timeUs_t currentTimeUs) posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal; } - /* If calibration is incomplete - report zero acceleration */ if (gravityCalibrationComplete()) { #ifdef USE_SIMULATOR if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) { @@ -432,7 +435,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; @@ -476,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 { @@ -503,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; } @@ -521,7 +526,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 */ @@ -552,20 +559,28 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; + 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; - 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; + 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 baro to prevent sudden jump - const float start_epv = positionEstimationConfig()->max_eph_epv; - const float end_epv = positionEstimationConfig()->max_eph_epv * 2.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; + + // 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)) { @@ -588,21 +603,25 @@ 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); + const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p); + 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, w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p); + ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); } - correctOK = true; + 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; @@ -611,19 +630,20 @@ 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); + const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; - 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, fabsf(gpsAltResudual)), positionEstimationConfig()->w_z_gps_p); + 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, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p); + ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); } - correctOK = true; + correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } return correctOK; @@ -700,21 +720,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); @@ -737,12 +759,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 @@ -754,16 +776,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; } } diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 44f0333d149..5cbdc81c030 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -146,6 +146,13 @@ typedef enum { EST_Z_VALID = (1 << 6), } navPositionEstimationFlags_e; +typedef enum { + ALTITUDE_SOURCE_GPS, + ALTITUDE_SOURCE_BARO, + ALTITUDE_SOURCE_GPS_ONLY, + ALTITUDE_SOURCE_BARO_ONLY, +} navDefaultAltitudeSensor_e; + typedef struct { timeUs_t baroGroundTimeout; float baroGroundAlt;