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 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..436a4d6fbd48e 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -28,17 +28,21 @@ 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); + 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)) { + _velocity_cm.z = -rate_z * 100; // convert from m/s in NED to cm/s in NEU + } + } } /** 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