diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 2216af40ef53037482d1833d76fd6438a59a3787..a63af018f245c652c4b4101cdecaf7944660b3be 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; }