mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
AP_NavEKF3: do not treat a never-written terrain timestamp as fresh
terrain_srtm_alt_ms is zero until the first writeTerrainData, and imuSampleTime_ms counts from boot, so for the first five seconds of uptime the age test passes against a terrain altitude that is still zero. terrain_srtm_alt_valid then reports a terrain height the core has never received: FuseOptFlow scales flow from it, getHeightControlLimit drops the optical flow altitude cap, and getFilterStatus counts it towards relative position validity. On the ground at boot that costs little, since the value it invents is the height above an origin the vehicle is sitting on. It matters after an in-flight watchdog reset, where millis() restarts at zero and the vehicle comes back armed and flying. gndHgtValidTime_ms is guarded against exactly this one expression away, so this is the same idiom rather than a new one. The 5 s is named while it is being touched, because it is about to have a second user. No autotest: the window is the first five seconds of uptime and closes before healthy() opens, so SITL cannot enter it from a cold boot, and the in-flight reset that reaches it is not something the harness can stage.
This commit is contained in:
@@ -308,7 +308,8 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
|
||||
#if EK3_FEATURE_OPTFLOW_SRTM
|
||||
// if ground offset (aka terrainState) is not valid, use SRTM altitude. terrain_srtm_alt
|
||||
// is positive up from the origin where pd and terrainState above are positive down
|
||||
terrain_srtm_alt_valid = ((imuSampleTime_ms - terrain_srtm_alt_ms) < 5000);
|
||||
terrain_srtm_alt_valid = (terrain_srtm_alt_ms != 0) &&
|
||||
((imuSampleTime_ms - terrain_srtm_alt_ms) < TERRAIN_SRTM_ALT_TIMEOUT_MS);
|
||||
if (!gndOffsetValid && terrain_srtm_alt_valid) {
|
||||
heightAboveGndEst = MAX((-pd) - terrain_srtm_alt, rngOnGnd);
|
||||
}
|
||||
|
||||
@@ -71,6 +71,9 @@
|
||||
// number of seconds a request to reset the yaw to the GSF estimate is active before it times out
|
||||
#define YAW_RESET_TO_GSF_TIMEOUT_MS 5000
|
||||
|
||||
// age at which a terrain altitude from the database is no longer used
|
||||
#define TERRAIN_SRTM_ALT_TIMEOUT_MS 5000
|
||||
|
||||
// accuracy threshold applied to GSF yaw estimate use
|
||||
#define GSF_YAW_ACCURACY_THRESHOLD_DEG 15.0f
|
||||
|
||||
|
||||
Reference in New Issue
Block a user