From fe9dde31021afbc0bb70324aabcd5e9996625be7 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 6 Nov 2023 17:25:35 -0500 Subject: [PATCH] AP_InertialNav: freeze horiz_vel when !velned_ok This addresses concerns about brief failures of get_velocity_NED causing abrupt changes to the horizonatl velocity estimate. --- libraries/AP_InertialNav/AP_InertialNav.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 7b0dcb56fcb087..a6a1510b487df5 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -28,19 +28,20 @@ void AP_InertialNav::update(bool high_vibes) // get the velocity relative to the local earth frame Vector3f velNED; + const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED); - /* - during high vibration events use vertical position change. If - get_velocity_NED() fails we use a zero horizontal velocity - */ + if (velned_ok) { + _velocity_cm = -velNED * 100; // convert from m/s in NED to cm/s in NEU + } + // During high vibration events, or failure of get_velocity_NED, use the + // fallback vertical velocity estimate. For get_velocity_NED failure, freeze + // the horizontal velocity at the last good value. if (!velned_ok || high_vibes) { float rate_z; if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) { - velNED.z = rate_z; + _velocity_cm.z = -rate_z * 100; // convert from m/s in NED to cm/s in NEU } } - _velocity_cm = velNED * 100; // convert to cm/s - _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU } /**