AP_AHRS: avoid FPE when we don't have a compass reading

2015-05-02 10:02:55 +10:00 · 2015-05-02 10:02:55 +10:00 · 463270e0ee
parent 77a2b4acf6
commit 463270e0ee
1 changed files with 4 additions and 0 deletions
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@ -271,6 +271,10 @@ AP_AHRS_DCM::yaw_error_compass(void)
    // get the mag vector in the earth frame
    Vector2f rb = _dcm_matrix.mulXY(mag);

+    if (rb.length() < FLT_EPSILON) {
+        return 0.0f;
+    }
+
    rb.normalize();
    if (rb.is_inf()) {
        // not a valid vector