AP_InertialNav: add fallback numeric vert velocity

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

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

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
+        }
+    }
 }
 
 /**

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