Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: keep baro ground effect out of a height source switch

Browse Source 
With EK3_RNG_USE_HGT, Copter uses a range finder that reads on the ground
for height while taking off, so the baro offset filter runs while the
motors spool up and learns the prop-wash baro error as offset. ALT_HOLD's
takeoff ends as the vehicle leaves the ground, the source goes back to baro
there, and the offset carries the error into the flight: in SITL with
SIM_BARO_GEFF_M 3 the EKF height read 2.5 m high from then until landing.

While takeoff or touchdown is expected, do not update the offset, and do
not reset the height to the baro on switching to it. Either alone is not
enough: with only the offset held, the reset drops the height by the whole
error at liftoff (3.0 m); with only the reset skipped, the offset still
carries it (2.5 m). With both, -0.17 m worst in the takeoff and 0.03 m mean
above 5 m.

Baro drift is not learned while the flags are set, which on Copter
includes the whole time it sits armed on the ground. Learning only the
upward part of the error was tried: in SITL it did no better, and baro
noise ratchets it upwards (0.07 m after a 10 s armed wait with
SIM_BARO_RND 0.2). Fixed wing sets the flags for a launch rather than for
rotor wash, so it is left as it was.
This commit is contained in:
Andy Piper
2026-09-23 18:47:38 +10:00
committed by Peter Barker
parent caadaf7daf
commit bb615f0f93
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+11 -3
View File
@@ -1334,10 +1334,15 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
}
// ground effect makes the baro read low while takeoff or touchdown is expected. Fixed wing
// sets those flags for a launch rather than for rotor wash, so it is left as it was
const bool baroInGndEffect = (dal.get_takeoff_expected() || dal.get_touchdown_expected()) && !assume_zero_sideslip();
// if there is new baro data to fuse, calculate filtered baro data required by other processes
if (baroDataToFuse) {
// calculate offset to baro data that enables us to switch to Baro height use during operation
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO) {
// calculate offset to baro data that enables us to switch to Baro height use during operation.
// Not from a baro in ground effect: the offset would carry the error into the next switch
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO && !baroInGndEffect) {
calcFiltBaroOffset();
}
// filtered baro data used to provide a reference for takeoff
@@ -1440,7 +1445,10 @@ void NavEKF3_core::selectHeightForFusion()
// detect changes in source and reset height
if ((activeHgtSource != prevHgtSource) && fuseHgtData) {
prevHgtSource = activeHgtSource;
ResetPositionD(-hgtMea);
// a baro in ground effect is no height to reset to, so a switch to it keeps the height held
if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO || !baroInGndEffect) {
ResetPositionD(-hgtMea);
}
}
// If we haven't fused height data for a while or have bad IMU data, then declare the height data as being timed out