rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-443 - disable Nav Alarm for non Nav algorithms

Browse Source
This commit is contained in:
Alessio Morale 2016-11-04 02:37:17 +01:00
parent 3eb4b45f04
commit c5dec236f7
3 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

@ -74,6 +74,7 @@ typedef struct {
float debugNavYaw;
uint8_t magStatus;
bool navOk;
bool navUsed;
bool armed;
sensorUpdates updated;
} stateEstimation;

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

@ -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,11 +601,15 @@ static void StateEstimationCb(void)
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
if (!states.navUsed) {
AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_UNINITIALISED);
} else {
if (states.navOk) {
AlarmsClear(SYSTEMALARMS_ALARM_NAV);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_NAV, SYSTEMALARMS_ALARM_CRITICAL);
}
}
if (updatedSensors) {
PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);