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.
This commit is contained in:
Bob Long 2023-11-06 17:25:35 -05:00 committed by Andrew Tridgell
parent 49dea0bd2e
commit 24bf288e28
1 changed files with 9 additions and 7 deletions

View File

@ -28,19 +28,21 @@ 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 to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to 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
}
/**