AP_Airspeed: Altitude Calibration - added constrain on P matrix diagonals

2013-07-20 17:16:07 +10:00 · 2013-07-20 17:16:07 +10:00 · 5a68c515dc
commit 5a68c515dc
parent 6e288551ab
1 changed files with 5 additions and 0 deletions
--- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp
+++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
@ -96,6 +96,11 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg)
 	P.a.z = P.c.x = P13;
 	P.b.z = P.c.y = P23;

+    // Constrain diagonals to be non-negative - protects against rounding errors
+    P.a.x = max(P.a.x, 0.0f);
+    P.b.y = max(P.b.y, 0.0f);
+    P.c.z = max(P.c.z, 0.0f);
+
    return state.z;
 }