mirror of https://github.com/ArduPilot/ardupilot
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:
parent
49dea0bd2e
commit
24bf288e28
|
@ -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
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue