Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

development of posestimator fusion

Browse source
This commit is contained in:
shota 2023-10-22 16:51:09 +09:00
parent b6fb8842d9
commit 8e3ee38e51
2 changed files with 11 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/fc/settings.yaml
View file

@ -2314,13 +2314,13 @@ groups:
max: 100 max: 100
default_value: 2.0 default_value: 2.0
- name: inav_w_z_baro_p - 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 field: w_z_baro_p
min: 0 min: 0
max: 10 max: 10
default_value: 0.35 default_value: 0.35
- name: inav_w_z_gps_p - 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 field: w_z_gps_p
min: 0 min: 0
max: 10 max: 10

16
src/main/navigation/navigation_pos_estimator.c
View file

@ -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, 5, posEstimator.gps.vel.z); // GPS vertical speed
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count 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(); timeUs_t currentTimeUs = micros();
if (!ARMING_FLAG(ARMED)) { 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) && bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || (((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)); ((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); 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 (ctx->newFlags & EST_GPS_Z_VALID) {
// If baro is not available - use GPS Z for correction on a plane
// Reset current estimate to GPS altitude if estimate not valid // Reset current estimate to GPS altitude if estimate not valid
if (!(ctx->newFlags & EST_Z_VALID)) { if (!(ctx->newFlags & EST_Z_VALID)) {
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; 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); 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) static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)