From 449d09051eba9f90b5cb4fc524faab9033224423 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 2 Mar 2014 14:32:10 +1100
Subject: [PATCH] AP_NavEKF: cope with compass going offline while in flight

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 14 +++++++++++---
 libraries/AP_NavEKF/AP_NavEKF.h   |  3 +++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 6b3f1e93fd..c4cff9cca3 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -363,7 +363,7 @@ void NavEKF::InitialiseFilterDynamic(void)
     float magHeading = atan2f(initMagNED.y, initMagNED.x);
 
     // get the magnetic declination
-    float magDecAng = _ahrs->get_compass()->get_declination();
+    float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 
     // calculate yaw angle rel to true north
     yaw = magDecAng - magHeading;
@@ -439,7 +439,7 @@ void NavEKF::InitialiseFilterBootstrap(void)
         float magHeading = atan2f(initMagNED.y, initMagNED.x);
 
         // get the magnetic declination
-        float magDecAng = _ahrs->get_compass()->get_declination();
+        float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
 
         // calculate yaw angle rel to true north
         yaw = magDecAng - magHeading;
@@ -2630,7 +2630,7 @@ void NavEKF::readHgtData()
 void NavEKF::readMagData()
 {
     // scale compass data to improve numerical conditioning
-    if (_ahrs->get_compass()->last_update != lastMagUpdate) {
+    if (use_compass() && _ahrs->get_compass()->last_update != lastMagUpdate) {
         lastMagUpdate = _ahrs->get_compass()->last_update;
         // Body fixed magnetic bias is opposite sign to APM compass offsets
         magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
@@ -2800,4 +2800,12 @@ bool NavEKF::static_mode_demanded(void) const
     return !_ahrs->get_armed() || !_ahrs->get_correct_centrifugal();
 }
 
+/*
+  see if we should use the compass
+ */
+bool NavEKF::use_compass(void) const
+{
+    return _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw();
+}
+
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
index b371e995f4..4be807724e 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.h
+++ b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -455,6 +455,9 @@ private:
     perf_counter_t  _perf_FuseMagnetometer;
     perf_counter_t  _perf_FuseAirspeed;
 #endif
+    
+    // should we use the compass?
+    bool use_compass(void) const;
 };
 
 #if CONFIG_HAL_BOARD != HAL_BOARD_PX4