From 3ee3a7164405f628ba446c6c57b1a079fa3e7556 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 8 Aug 2014 15:55:20 +1000
Subject: [PATCH] AP_NavEKF: prevent a possible numerical error on startup

fixes issue #1294
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 1ef0a46c1c..931d20a17c 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -511,13 +511,16 @@ void NavEKF::InitialiseFilterBootstrap(void)
     readMagData();
 
     // normalise the acceleration vector
-    initAccVec.normalize();
+    float pitch=0, roll=0;
+    if (initAccVec.length() > 0.001f) {
+        initAccVec.normalize();
 
-    // calculate initial pitch angle
-    float pitch = asinf(initAccVec.x);
+        // calculate initial pitch angle
+        pitch = asinf(initAccVec.x);
 
-    // calculate initial roll angle
-    float roll = -asinf(initAccVec.y / cosf(pitch));
+        // calculate initial roll angle
+        roll = -asinf(initAccVec.y / cosf(pitch));
+    }
 
     // calculate initial orientation and earth magnetic field states
     Quaternion initQuat;