From 5a68c515dc0ce4e1ec5493b6134ce8191a15dbbe Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 20 Jul 2013 17:16:07 +1000
Subject: [PATCH] AP_Airspeed: Altitude Calibration - added constrain on P
 matrix diagonals

---
 libraries/AP_Airspeed/Airspeed_Calibration.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp
index 2101d1bba5..d57948f20f 100644
--- 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;
 }