Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialNav: freeze horiz_vel when !velned_ok

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -28,19 +28,21 @@ void AP_InertialNav::update(bool high_vibes)
// get the velocity relative to the local earth frame // get the velocity relative to the local earth frame
Vector3f velNED; Vector3f velNED;
const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED); const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED);
/* if (velned_ok) {
during high vibration events use vertical position change. If _velocity_cm = velNED * 100; // convert to cm/s
get_velocity_NED() fails we use a zero horizontal velocity _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) { if (!velned_ok || high_vibes) {
float rate_z; float rate_z;
if (_ahrs_ekf.get_vert_pos_rate_D(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
} }
/** /**