ekf2: rng only reset vz as a last resort

This commit is contained in:
Daniel Agar
2024-07-15 11:39:56 -04:00
parent 80ee622f77
commit 8e5f28f834
2 changed files with 6 additions and 4 deletions
@@ -180,8 +180,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_information_events.flags.reset_hgt_to_rng = true;
resetVerticalPositionTo(-(aid_src.observation - _state.terrain));
// reset vertical velocity
resetVerticalVelocityToZero();
// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = imu_sample.time_us;