From b3c1e515dc43d98460253b9de0c00b17fbc33a6c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 15 May 2014 22:19:54 +1000
Subject: [PATCH] AP_AHRS: when no accel info available in buffer use current
 value

this prevents an initialisation error, and is reasonable in flight too

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
---
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 2216af40e..a63af018f 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -476,6 +476,11 @@ Vector3f AP_AHRS_DCM::ra_delayed(uint8_t instance, const Vector3f &ra)
     // get the old element, and then fill it with the new element
     Vector3f ret = _ra_delay_buffer[instance];
     _ra_delay_buffer[instance] = ra;
+    if (ret.is_zero()) {
+        // use the current vector if the previous vector is exactly
+        // zero. This prevents an error on initialisation
+        return ra;
+    }
     return ret;
 }
 
-- 
GitLab