From 2888bdd6d5e29b3c407851326fd098c1f2e10dc6 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Tue, 1 Mar 2016 17:51:04 +1100
Subject: [PATCH] AP_NavEKF: Protect against possible div by 0

---
 libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 28 +++++++++----------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
index 543b16cf0f..98080ee997 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
@@ -628,7 +628,12 @@ void NavEKF2_core::fuseEulerYaw()
         float t7 = q0*q3*2.0f;
         float t8 = q1*q2*2.0f;
         float t9 = t7+t8;
-        float t10 = 1.0f/(t6*t6);
+        float t10 = sq(t6);
+        if (t10 > 1e-6f) {
+            t10 = 1.0f / t10;
+        } else {
+            return;
+        }
         float t11 = t9*t9;
         float t12 = t10*t11;
         float t13 = t12+1.0f;
@@ -638,12 +643,7 @@ void NavEKF2_core::fuseEulerYaw()
         } else {
             return;
         }
-        float t15;
-        if (fabsf(t6) > 1e-6) {
-            t15 = 1.0f/t6;
-        } else {
-            return;
-        }
+        float t15 = 1.0f/t6;
         H_YAW[0] = 0.0f;
         H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
         H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
@@ -666,7 +666,12 @@ void NavEKF2_core::fuseEulerYaw()
         float t7 = q0*q3*2.0f;
         float t10 = q1*q2*2.0f;
         float t8 = t7-t10;
-        float t9 = 1.0f/(t6*t6);
+        float t9 = sq(t6);
+        if (t9 > 1e-6f) {
+            t9 = 1.0f/t9;
+        } else {
+            return;
+        }
         float t11 = t8*t8;
         float t12 = t9*t11;
         float t13 = t12+1.0f;
@@ -676,12 +681,7 @@ void NavEKF2_core::fuseEulerYaw()
         } else {
             return;
         }
-        float t15;
-        if (fabsf(t6) > 1e-6) {
-            t15 = 1.0f/t6;
-        } else {
-            return;
-        }
+        float t15 = 1.0f/t6;
         H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
         H_YAW[1] = 0.0f;
         H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));