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