From e334a5dc57d21ca601f471d3d8ccdafbb3fb7731 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Wed, 16 Mar 2016 13:43:21 +1100
Subject: [PATCH] EKF: Add check for NaN's on attitude states

---
 EKF/ekf.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index ffe593f924..0f37410801 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -307,6 +307,11 @@ bool Ekf::update()
 	// the output observer always runs
 	calculateOutputStates();
 
+	// check for NaN on attitude states
+	if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) {
+		return false;
+	}
+
 	// We don't have valid data to output until tilt and yaw alignment is complete
 	if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
 		return true;