aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialNav: add fallback vertical velocity

Browse Source
This commit is contained in:
Bob Long 2023-10-25 18:50:02 -04:00 committed by Andrew Tridgell
parent 60816f4351
commit 6eed40dec1
2 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -1205,11 +1205,14 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const
{
Vector3f velned;
if (!get_velocity_NED(velned)) {
return false;
if (get_velocity_NED(velned)) {
velocity = velned.z;
return true;
} else if (AP::baro().healthy()) {
velocity = -AP::baro().get_climb_rate();
return true;
}
velocity = velned.z;
return true;
return false;
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message

20
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -28,17 +28,19 @@ void AP_InertialNav::update(bool high_vibes)
// get the velocity relative to the local earth frame
Vector3f velNED;
if (_ahrs_ekf.get_velocity_NED(velNED)) {
// during high vibration events use vertical position change
if (high_vibes) {
float rate_z;
if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) {
velNED.z = rate_z;
}
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 || high_vibes) {
float rate_z;
if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) {
velNED.z = rate_z;
}
_velocity_cm = velNED * 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
}
_velocity_cm = velNED * 100; // convert to cm/s
_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
}
/**