From 93dcc55977d0081927facb9f7f0f6b38c0e96b60 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 23 Sep 2016 01:19:16 +0200 Subject: [PATCH] LP-353 - Introduce a Nav alarm that is active when Position estimation is not available --- flight/libraries/alarms.c | 1 + flight/modules/StateEstimation/filterekf.c | 14 ++++---- .../StateEstimation/inc/stateestimation.h | 1 + .../modules/StateEstimation/stateestimation.c | 36 +++++++++++++++---- flight/pios/inc/pios_math.h | 2 +- shared/uavobjectdefinition/systemalarms.xml | 1 + 6 files changed, 40 insertions(+), 15 deletions(-) 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