diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c
index 7cc7f39aa..b9a977a8b 100644
--- a/flight/libraries/alarms.c
+++ b/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",
diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c
index 199f984fd..6cf7027da 100644
--- a/flight/modules/StateEstimation/filterekf.c
+++ b/flight/modules/StateEstimation/filterekf.c
@@ -90,8 +90,8 @@ static int32_t globalInit(stateFilter *handle, bool usePos, bool navOnly)
handle->filter = &filter;
handle->localdata = pios_malloc(sizeof(struct data));
struct data *this = (struct data *)handle->localdata;
- this->usePos = usePos;
- this->navOnly = navOnly;
+ this->usePos = usePos;
+ this->navOnly = navOnly;
EKFConfigurationInitialize();
EKFStateVarianceInitialize();
HomeLocationInitialize();
@@ -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;
}
diff --git a/flight/modules/StateEstimation/inc/stateestimation.h b/flight/modules/StateEstimation/inc/stateestimation.h
index 7de46bb10..3278abe3f 100644
--- a/flight/modules/StateEstimation/inc/stateestimation.h
+++ b/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;
diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c
index 9d2afd09f..6d19a2368 100644
--- a/flight/modules/StateEstimation/stateestimation.c
+++ b/flight/modules/StateEstimation/stateestimation.c
@@ -398,13 +398,16 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
*/
static void StateEstimationCb(void)
{
- static filterResult alarm = FILTERRESULT_OK;
- static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
- static uint16_t alarmcounter = 0;
+ 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) {
@@ -550,10 +554,10 @@ static void StateEstimationCb(void)
if (IS_SET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
- s.q1 = states.attitude[0];
- s.q2 = states.attitude[1];
- s.q3 = states.attitude[2];
- s.q4 = states.attitude[3];
+ s.q1 = states.attitude[0];
+ s.q2 = states.attitude[1];
+ s.q3 = states.attitude[2];
+ s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll);
s.NavYaw = states.debugNavYaw;
AttitudeStateSet(&s);
@@ -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 {
diff --git a/flight/pios/inc/pios_math.h b/flight/pios/inc/pios_math.h
index 8cc57cad5..8c89b13c5 100644
--- a/flight/pios/inc/pios_math.h
+++ b/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))
diff --git a/shared/uavobjectdefinition/systemalarms.xml b/shared/uavobjectdefinition/systemalarms.xml
index 7cf9d55ed..3615465d7 100644
--- a/shared/uavobjectdefinition/systemalarms.xml
+++ b/shared/uavobjectdefinition/systemalarms.xml
@@ -14,6 +14,7 @@
ManualControl
Actuator
Attitude
+ Nav
Sensors
Magnetometer
Airspeed