From 5bd333fd97e0c8a3854f07d753030f5ab8b95a6a Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 25 Oct 2023 18:50:02 -0400 Subject: [PATCH 1/4] AP_InertialNav: add fallback vertical velocity --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 11 +++++++---- libraries/AP_InertialNav/AP_InertialNav.cpp | 20 +++++++++++--------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index df722c5cc52b0..5dd7f681ba195 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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 diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 9e0aa38a57af9..7b0dcb56fcb08 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -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 } /** From 49bfa8aea3ca01572c471e6ae147326a1f5ba2f3 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 30 Oct 2023 20:46:38 -0400 Subject: [PATCH 2/4] GCS_Common: allow fallback vert rate in VFR_HUD --- libraries/GCS_MAVLink/GCS_Common.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 053e7294aaeee..a81f3d5a38cb7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3111,10 +3111,11 @@ float GCS_MAVLINK::vfr_hud_airspeed() const float GCS_MAVLINK::vfr_hud_climbrate() const { Vector3f velned; - if (!AP::ahrs().get_velocity_NED(velned)) { - velned.zero(); + if (AP::ahrs().get_velocity_NED(velned) || + AP::ahrs().get_vert_pos_rate_D(velned.z)) { + return -velned.z; } - return -velned.z; + return 0; } float GCS_MAVLINK::vfr_hud_alt() const From f39b56004879a3e6ecf6862e2e8462b9fe05d35c Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 30 Oct 2023 20:50:27 -0400 Subject: [PATCH 3/4] autotest: add DCM climb rate test for quadplanes --- Tools/autotest/quadplane.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index d2e61b4f07ce0..ebb0c2f9cf859 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1513,6 +1513,41 @@ def Q_GUIDED_MODE(self): self.fly_home_land_and_disarm() + def DCMClimbRate(self): + '''Test the climb rate measurement in DCM with and without GPS''' + self.wait_ready_to_arm() + + self.change_mode('QHOVER') + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(30, 50, relative=True) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Switch to DCM + self.set_parameter('AHRS_EKF_TYPE', 0) + self.delay_sim_time(5) + + # Start Climbing + self.set_rc(3, 2000) + self.wait_climbrate(0.5, 5, timeout=10) + + # Kill any GPSs + self.set_parameters({ + 'SIM_GPS_DISABLE': 1, + 'SIM_GPS2_DISABLE': 1, + }) + self.delay_sim_time(5) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Force disarm + self.disarm_vehicle(force=True) + def tests(self): '''return list of all tests''' @@ -1554,5 +1589,6 @@ def tests(self): self.mavlink_MAV_CMD_DO_VTOL_TRANSITION, self.MAV_CMD_NAV_TAKEOFF, self.Q_GUIDED_MODE, + self.DCMClimbRate, ]) return ret From 933dba137676bbbb91c8607bd27e993b31423587 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Mon, 6 Nov 2023 17:25:35 -0500 Subject: [PATCH 4/4] 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. --- libraries/AP_InertialNav/AP_InertialNav.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 7b0dcb56fcb08..436a4d6fbd48e 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -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 } /**