AP_InertialNav: add fallback vertical velocity

ArduPilot · Oct 25, 2023 · bf90399 · bf90399
1 parent 62af92e
commit bf90399
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 12 deletions.
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -1205,10 +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;
+    }
+    else
+    {
+        velocity = -AP::baro().get_climb_rate();
     }
-    velocity = velned.z;
+
     return true;
 }
 

diff --git 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
 }
 
 /**