From c5dec236f7ad9292e07dda576e17d275468b617f Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Fri, 4 Nov 2016 02:37:17 +0100
Subject: [PATCH] LP-443 - disable Nav Alarm for non Nav algorithms

---
 flight/modules/StateEstimation/filterekf.c           |  2 +-
 flight/modules/StateEstimation/inc/stateestimation.h |  1 +
 flight/modules/StateEstimation/stateestimation.c     | 11 ++++++++---
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index 6cf7027da..b0cb714b3 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -184,7 +184,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
     uint16_t sensors = 0;
 
     INSSetArmed(state->armed);
-
+    state->navUsed      = (this->usePos || this->navOnly);
     this->work.updated |= state->updated;
     // check magnetometer alarm, discard any magnetometer readings if not OK
     // during initialization phase (but let them through afterwards)
diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index 3278abe3f..ad71c7c18 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/flight/modules/StateEstimation/inc/stateestimation.h
@@ -74,6 +74,7 @@ typedef struct {
     float   debugNavYaw;
     uint8_t magStatus;
     bool    navOk;
+    bool    navUsed;
     bool    armed;
     sensorUpdates updated;
 } stateEstimation;
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index 6d19a2368..ad06cba0e 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -465,6 +465,7 @@ static void StateEstimationCb(void)
             bool error = 0;
             states.debugNavYaw = 0;
             states.navOk = false;
+            states.navUsed     = false;
             while (current != NULL) {
                 int32_t result = current->filter->init((stateFilter *)current->filter);
                 if (result != 0) {
@@ -600,10 +601,14 @@ static void StateEstimationCb(void)
         AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
     }
 
-    if (states.navOk) {
-        AlarmsClear(SYSTEMALARMS_ALARM_NAV);
+    if (!states.navUsed) {
+        AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_UNINITIALISED);
     } else {
-        AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL);
+        if (states.navOk) {
+            AlarmsClear(SYSTEMALARMS_ALARM_NAV);
+        } else {
+            AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL);
+        }
     }
 
     if (updatedSensors) {