LP-353 - Introduce a Nav alarm that is active when Position estimation is not available

flight/libraries/alarms.c

@@ -307,6 +307,7 @@ static const char *const systemalarms_alarm_names[] = {
     [SYSTEMALARMS_ALARM_MAGNETOMETER]  = "MAG",
     [SYSTEMALARMS_ALARM_AIRSPEED]      = "AIRSPD",
     [SYSTEMALARMS_ALARM_STABILIZATION] = "STAB",
+    [SYSTEMALARMS_ALARM_NAV] = "NAV",
     [SYSTEMALARMS_ALARM_GUIDANCE]      = "GUIDANCE",
     [SYSTEMALARMS_ALARM_PATHPLAN]      = "PLAN",
     [SYSTEMALARMS_ALARM_BATTERY]       = "BATT",

flight/modules/StateEstimation/filterekf.c

@@ -284,7 +284,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
             // Copy the attitude into the state
             // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
-            if(!this->navOnly){
+            if (!this->navOnly) {
                 state->attitude[0] = Nav.q[0];
                 state->attitude[1] = Nav.q[1];
                 state->attitude[2] = Nav.q[2];
@@ -305,6 +305,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
         this->init_stage++;
         if (this->init_stage > 10) {
+            state->navOk = true;
             this->inited = true;
         }
 
@@ -312,7 +313,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
     }
 
     if (!this->inited) {
-        return FILTERRESULT_CRITICAL;
+        return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_CRITICAL;
     }
 
     float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
@@ -322,8 +323,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
 
     // Copy the attitude into the state
     // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
-    if(!this->navOnly){
-
+    if (!this->navOnly) {
         state->attitude[0] = Nav.q[0];
         state->attitude[1] = Nav.q[1];
         state->attitude[2] = Nav.q[2];
@@ -428,7 +428,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
     this->work.updated = 0;
 
     if (this->init_stage < 0) {
-        return FILTERRESULT_WARNING;
+        return this->navOnly ? FILTERRESULT_OK : FILTERRESULT_WARNING;
     } else {
         return FILTERRESULT_OK;
     }

flight/modules/StateEstimation/inc/stateestimation.h

@@ -73,6 +73,7 @@ typedef struct {
     float   boardMag[3];
     float   debugNavYaw;
     uint8_t magStatus;
+    bool    navOk;
     bool    armed;
     sensorUpdates updated;
 } stateEstimation;

flight/modules/StateEstimation/stateestimation.c

@@ -400,11 +400,14 @@ static void StateEstimationCb(void)
 {
     static filterResult alarm        = FILTERRESULT_OK;
     static filterResult lastAlarm    = FILTERRESULT_UNINITIALISED;
+    static bool lastNavStatus        = false;
     static uint16_t alarmcounter     = 0;
+    static uint16_t navstatuscounter = 0;
     static const filterPipeline *current;
     static stateEstimation states;
     static uint32_t last_time;
     static uint16_t bootDelay = 64;
+
     // after system startup, first few sensor readings might be messed up, delay until everything has settled
     if (bootDelay) {
         bootDelay--;
@@ -461,6 +464,7 @@ static void StateEstimationCb(void)
             current = newFilterChain;
             bool error = 0;
             states.debugNavYaw = 0;
+            states.navOk = false;
             while (current != NULL) {
                 int32_t result = current->filter->init((stateFilter *)current->filter);
                 if (result != 0) {
@@ -573,6 +577,18 @@ static void StateEstimationCb(void)
         }
     }
 
+    if (lastNavStatus < states.navOk) {
+        lastNavStatus    = states.navOk;
+        navstatuscounter = 0;
+    } else {
+        if (navstatuscounter < 100) {
+            navstatuscounter++;
+        } else {
+            lastNavStatus    = states.navOk;
+            navstatuscounter = 0;
+        }
+    }
+
     // clear alarms if everything is alright, then schedule callback execution after timeout
     if (lastAlarm == FILTERRESULT_WARNING) {
         AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
@@ -584,6 +600,12 @@ static void StateEstimationCb(void)
         AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
     }
 
+    if (states.navOk) {
+        AlarmsClear(SYSTEMALARMS_ALARM_NAV);
+    } else {
+        AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL);
+    }
+
     if (updatedSensors) {
         PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
     } else {

flight/pios/inc/pios_math.h

@@ -79,7 +79,7 @@
 #define MAX(a, b)               ((a) > (b) ? (a) : (b))
 #define MIN(a, b)               ((a) < (b) ? (a) : (b))
 
-#define IS_REAL(f)              (!isnan(f) && !isinf(f))
+#define IS_REAL(f)              (isfinite(f))
 
 // Bitfield access
 #define IS_SET(field, mask)     (((field) & (mask)) == (mask))

shared/uavobjectdefinition/systemalarms.xml

@@ -14,6 +14,7 @@
 			<elementname>ManualControl</elementname>
 			<elementname>Actuator</elementname>
 			<elementname>Attitude</elementname>
+			<elementname>Nav</elementname>
 			<elementname>Sensors</elementname>
 			<elementname>Magnetometer</elementname>
 			<elementname>Airspeed</elementname>