rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

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

Browse Source
This commit is contained in:
Alessio Morale 2016-09-23 01:19:16 +02:00
parent 8df3758104
commit 93dcc55977
6 changed files with 40 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/alarms.c
View File

@ -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",

14
flight/modules/StateEstimation/filterekf.c
View File

@ -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;
}

1
flight/modules/StateEstimation/inc/stateestimation.h
View File

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

36
flight/modules/StateEstimation/stateestimation.c
View File

@ -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 {

2
flight/pios/inc/pios_math.h
View File

@ -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))

1
shared/uavobjectdefinition/systemalarms.xml
View File

@ -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>