Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues 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 bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const
{ {
Vector3f velned; Vector3f velned;
if (!get_velocity_NED(velned)) { if (get_velocity_NED(velned)) {
return false; velocity = velned.z;
return true;
} else if (AP::baro().healthy()) {
velocity = -AP::baro().get_climb_rate();
return true;
} }
velocity = velned.z; return false;
return true;
} }
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message // 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 // get the velocity relative to the local earth frame
Vector3f velNED; Vector3f velNED;
if (_ahrs_ekf.get_velocity_NED(velNED)) { const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED);
// during high vibration events use vertical position change /*
if (high_vibes) { during high vibration events use vertical position change. If
float rate_z; get_velocity_NED() fails we use a zero horizontal velocity
if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) { */
velNED.z = rate_z; 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
} }
/** /**