dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2-agp: reset to measurement on fusion timeout

This commit is contained in:
bresch 2024-01-19 14:21:46 +01:00 committed by Daniel Agar
parent 23ae769e46
commit 4268759d4a
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/ekf2/EKF/aux_global_position.cpp
View File

@ -119,6 +119,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
if (continuing_conditions) { if (continuing_conditions) {
ekf.fuseHorizontalPosition(aid_src); ekf.fuseHorizontalPosition(aid_src);
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)) {
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
aid_src.time_last_fuse = imu_delayed.time_us;
}
} else { } else {
ekf.disableControlStatusAuxGpos(); ekf.disableControlStatusAuxGpos();
_state = State::stopped; _state = State::stopped;