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

OP-1308 Set the same logic for alarms

Browse Source
This commit is contained in:
Bertrand Oresve 2014-04-29 22:25:49 +02:00
parent 333ccc7e42
commit 6d3ade4947
18 changed files with 120 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
flight/libraries/sanitycheck.c
View File

@ -94,7 +94,7 @@ int32_t configuration_check()
switch (modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
@ -108,51 +108,51 @@ int32_t configuration_check()
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports Position Hold
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports AutoLand Mode
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports POI Mode
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
@ -160,21 +160,21 @@ int32_t configuration_check()
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
}
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
break;
default:
// Uncovered modes are automatically an error
severity = SYSTEMALARMS_ALARM_ERROR;
severity = SYSTEMALARMS_ALARM_CRITICAL;
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
@ -197,14 +197,14 @@ int32_t configuration_check()
* Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_CRITICAL
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) {
return SYSTEMALARMS_ALARM_ERROR;
return SYSTEMALARMS_ALARM_CRITICAL;
}
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
@ -221,14 +221,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor)
FlightModeSettingsStabilization3SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
return SYSTEMALARMS_ALARM_CRITICAL;
}
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for (uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) {
return SYSTEMALARMS_ALARM_ERROR;
return SYSTEMALARMS_ALARM_CRITICAL;
}
}
}

4
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -192,7 +192,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
}
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
} else {
pathStatus.UID = pathDesired.UID;
@ -220,7 +220,7 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
}

2
flight/modules/ManualControl/armhandler.c
View File

@ -258,7 +258,7 @@ static bool okToArm(void)
// Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_CRITICAL) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue;
}

2
flight/modules/PathPlanner/pathplanner.c
View File

@ -185,7 +185,7 @@ static void pathPlannerTask()
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
return;
}

2
flight/modules/Stabilization/stabilization.c
View File

@ -638,7 +638,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Clear or set alarms. Done like this to prevent toggline each cycle
// and hammering system alarms
if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}

6
flight/modules/StateEstimation/filterair.c
View File

@ -50,7 +50,7 @@ struct data {
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterAirInitialize(stateFilter *handle)
@ -69,7 +69,7 @@ static int32_t init(stateFilter *self)
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
@ -82,7 +82,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}
return 0;
return FILTERRESULT_OK;
}

77
flight/modules/StateEstimation/filteraltitude.c
View File

@ -38,23 +38,16 @@
// Private constants
// duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
static volatile bool reloadSettings;
// Private types
struct data {
float altitudeState; // state = altitude,velocity,accel_offset,accel
float velocityState;
float accelBiasState;
float accelState;
float state[4]; // state = altitude,velocity,accel_offset,accel
float pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters
@ -63,7 +56,6 @@ struct data {
float accelLast;
float baroLast;
bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings;
};
@ -72,8 +64,7 @@ struct data {
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void settingsUpdatedCb(UAVObjEvent *ev);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterAltitudeInitialize(stateFilter *handle)
@ -83,8 +74,6 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->localdata = pvPortMalloc(sizeof(struct data));
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
reloadSettings = true;
return STACK_REQUIRED;
}
@ -92,10 +81,10 @@ static int32_t init(stateFilter *self)
{
struct data *this = (struct data *)self->localdata;
this->altitudeState = 0.0f;
this->velocityState = 0.0f;
this->accelBiasState = 0.0f;
this->accelState = 0.0f;
this->state[0] = 0.0f;
this->state[1] = 0.0f;
this->state[2] = 0.0f;
this->state[3] = 0.0f;
this->pos[0] = 0.0f;
this->pos[1] = 0.0f;
this->pos[2] = 0.0f;
@ -107,23 +96,18 @@ static int32_t init(stateFilter *self)
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
AltitudeFilterSettingsGet(&this->settings);
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (reloadSettings) {
reloadSettings = false;
AltitudeFilterSettingsGet(&this->settings);
}
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->initTimer = xTaskGetTickCount();
}
} else {
// save existing position and velocity updates so GPS will still work
@ -131,13 +115,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->pos[0] = state->pos[0];
this->pos[1] = state->pos[1];
this->pos[2] = state->pos[2];
state->pos[2] = -this->altitudeState;
state->pos[2] = -this->state[0];
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->vel[0] = state->vel[0];
this->vel[1] = state->vel[1];
this->vel[2] = state->vel[2];
state->vel[2] = -this->velocityState;
state->vel[2] = -this->state[1];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
@ -148,67 +132,58 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f);
// low pass filter accelerometers
this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
// allow the offset to reach quickly the target value in case of small AccelDriftKi
this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
} else {
// correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
}
this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current;
// correct accel offset (low pass zeroing)
this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3];
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->velocityState;
float speedLast = this->state[1];
this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
this->accelLast = this->accelState - this->accelBiasState;
this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT;
this->accelLast = this->state[3] - this->state[2];
this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
this->state[0] += 0.5f * (speedLast + this->state[1]) * dT;
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->altitudeState;
state->pos[2] = -this->state[0];
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->velocityState;
state->vel[2] = -this->state[1];
state->updated |= SENSORUPDATES_vel;
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass)
this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0];
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->altitudeState;
state->pos[2] = -this->state[0];
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->velocityState;
state->vel[2] = -this->state[1];
state->updated |= SENSORUPDATES_vel;
}
}
return 0;
return FILTERRESULT_OK;
}
void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev->obj == AltitudeFilterSettingsHandle()) {
reloadSettings = true;
}
}
/**
* @}

9
flight/modules/StateEstimation/filterbaro.c
View File

@ -56,7 +56,7 @@ struct data {
static int32_t initwithgps(stateFilter *self);
static int32_t initwithoutgps(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterBaroInitialize(stateFilter *handle)
@ -105,7 +105,7 @@ static int32_t maininit(stateFilter *self)
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
@ -128,7 +128,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
// make sure we raise an error until properly initialized - would not be good if people arm and
// use altitudehold without initialized barometer filter
return 2;
// Here, the filter is not initialized, return ERROR
return FILTERRESULT_CRITICAL;
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
@ -141,7 +142,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->baroAlt = state->baro[0];
state->baro[0] -= this->baroOffset;
}
return 0;
return FILTERRESULT_OK;
}
}

28
flight/modules/StateEstimation/filtercf.c
View File

@ -81,8 +81,8 @@ static FlightStatusData flightStatus;
static int32_t initwithmag(stateFilter *self);
static int32_t initwithoutmag(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static filterResult filter(stateFilter *self, stateEstimation *state);
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4]);
static void flightStatusUpdatedCb(UAVObjEvent *ev);
@ -165,11 +165,11 @@ static int32_t maininit(stateFilter *self)
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
int32_t result = 0;
filterResult result = FILTERRESULT_OK;
if (IS_SET(state->updated, SENSORUPDATES_mag)) {
this->magUpdated = 1;
@ -187,7 +187,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
if (!result) {
if (result == FILTERRESULT_OK) {
state->attitude[0] = attitude[0];
state->attitude[1] = attitude[1];
state->attitude[2] = attitude[2];
@ -215,7 +215,7 @@ static inline void apply_accel_filter(const struct data *this, const float *raw,
}
}
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
static filterResult complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
{
float dT;
@ -224,7 +224,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
#if defined(PIOS_INCLUDE_HMC5883)
// wait until mags have been updated
if (!this->magUpdated) {
return 1;
return FILTERRESULT_ERROR;
}
#else
mag[0] = 100.0f;
@ -280,13 +280,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
this->timeval = PIOS_DELAY_GetRaw(); // Cycle counter used for precise timing
this->starttime = xTaskGetTickCount(); // Tick counter used for long time intervals
return 0; // must return zero on initial initialization, so attitude will init with a valid quaternion
return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion
}
if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
// wait 4 seconds for the user to get his hands off in case the board was just powered
this->timeval = PIOS_DELAY_GetRaw();
return 1;
return FILTERRESULT_ERROR;
} else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) {
// For first 6 seconds use accels to get gyro bias
this->attitudeSettings.AccelKp = 1.0f;
@ -343,7 +343,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// Account for accel magnitude
float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
if (accel_mag < 1.0e-3f) {
return 2; // safety feature copied from CC
return FILTERRESULT_CRITICAL; // safety feature copied from CC
}
// Account for filtered gravity vector magnitude
@ -354,7 +354,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
grot_mag = 1.0f;
}
if (grot_mag < 1.0e-3f) {
return 2;
return FILTERRESULT_CRITICAL;
}
accel_err[0] /= (accel_mag * grot_mag);
@ -447,13 +447,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
this->first_run = 1;
return 2;
return FILTERRESULT_WARNING;
}
if (this->init) {
return 0;
return FILTERRESULT_OK;
} else {
return 2; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later
return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later
}
}

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

@ -82,7 +82,7 @@ static bool initialized = 0;
static int32_t init13i(stateFilter *self);
static int32_t init13(stateFilter *self);
static int32_t maininit(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
static inline bool invalid_var(float data);
static void globalInit(void);
@ -192,7 +192,7 @@ static int32_t maininit(stateFilter *self)
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
@ -221,7 +221,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
UNSET_MASK(state->updated, SENSORUPDATES_vel);
UNSET_MASK(state->updated, SENSORUPDATES_attitude);
UNSET_MASK(state->updated, SENSORUPDATES_gyro);
return 0;
return FILTERRESULT_OK;
}
dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig);
@ -315,11 +315,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->inited = true;
}
return 0;
return FILTERRESULT_OK;
}
if (!this->inited) {
return 3;
return FILTERRESULT_CRITICAL;
}
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
@ -427,9 +427,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->work.updated = 0;
if (this->init_stage < 0) {
return 1;
return FILTERRESULT_WARNING;
} else {
return 0;
return FILTERRESULT_OK;
}
}

8
flight/modules/StateEstimation/filterlla.c
View File

@ -53,7 +53,7 @@ struct data {
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterLLAInitialize(stateFilter *handle)
@ -86,13 +86,13 @@ static int32_t init(__attribute__((unused)) stateFilter *self)
return 0;
}
static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
// cannot update local NED if home location is unset
if (this->home.Set != HOMELOCATION_SET_TRUE) {
return 0;
return FILTERRESULT_WARNING;
}
// only do stuff if we have a valid GPS update
@ -116,7 +116,7 @@ static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation
}
}
return 0;
return FILTERRESULT_OK;
}
/**

6
flight/modules/StateEstimation/filtermag.c
View File

@ -53,7 +53,7 @@ struct data {
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -76,7 +76,7 @@ static int32_t init(stateFilter *self)
return 0;
}
static int32_t filter(stateFilter *self, stateEstimation *state)
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
@ -86,7 +86,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
}
return 0;
return FILTERRESULT_OK;
}
/**

6
flight/modules/StateEstimation/filterstationary.c
View File

@ -42,7 +42,7 @@
// Private functions
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static filterResult filter(stateFilter *self, stateEstimation *state);
int32_t filterStationaryInitialize(stateFilter *handle)
@ -58,7 +58,7 @@ static int32_t init(__attribute__((unused)) stateFilter *self)
return 0;
}
static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{
state->pos[0] = 0.0f;
state->pos[1] = 0.0f;
@ -70,7 +70,7 @@ static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation
state->vel[2] = 0.0f;
state->updated |= SENSORUPDATES_vel;
return 0;
return FILTERRESULT_OK;
}
/**

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

@ -32,6 +32,17 @@
#include <openpilot.h>
// Enumeration for filter result
typedef enum {
FILTERRESULT_UNINITIALISED=-1,
FILTERRESULT_OK=0,
FILTERRESULT_WARNING=1,
FILTERRESULT_CRITICAL=2,
FILTERRESULT_ERROR=3,
} filterResult;
typedef enum {
SENSORUPDATES_gyro = 1 << 0,
SENSORUPDATES_accel = 1 << 1,
@ -58,7 +69,7 @@ typedef struct {
typedef struct stateFilterStruct {
int32_t (*init)(struct stateFilterStruct *self);
int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state);
filterResult (*filter)(struct stateFilterStruct *self, stateEstimation *state);
void *localdata;
} stateFilter;

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

@ -223,6 +223,7 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
}
}
};
static const filterPipeline *ekf13Queue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
@ -333,8 +334,8 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart);
static void StateEstimationCb(void)
{
static enum { RUNSTATE_LOAD = 0, RUNSTATE_FILTER = 1, RUNSTATE_SAVE = 2 } runState = RUNSTATE_LOAD;
static int8_t alarm = 0;
static int8_t lastAlarm = -1;
static filterResult alarm = FILTERRESULT_OK;
static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
static uint16_t alarmcounter = 0;
static const filterPipeline *current;
static stateEstimation states;
@ -351,12 +352,12 @@ static void StateEstimationCb(void)
switch (runState) {
case RUNSTATE_LOAD:
alarm = 0;
alarm = FILTERRESULT_OK;
// set alarm to warning if called through timeout
if (updatedSensors == 0) {
if (PIOS_DELAY_DiffuS(last_time) > 1000 * TIMEOUT_MS) {
alarm = 1;
alarm = FILTERRESULT_WARNING;
}
} else {
last_time = PIOS_DELAY_GetRaw();
@ -436,7 +437,7 @@ static void StateEstimationCb(void)
case RUNSTATE_FILTER:
if (current != NULL) {
int32_t result = current->filter->filter((stateFilter *)current->filter, &states);
filterResult result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
}
@ -487,12 +488,12 @@ static void StateEstimationCb(void)
}
// clear alarms if everything is alright, then schedule callback execution after timeout
if (lastAlarm == 1) {
if (lastAlarm == FILTERRESULT_WARNING) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
} else if (lastAlarm == 2) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else if (lastAlarm >= 3) {
} else if (lastAlarm == FILTERRESULT_CRITICAL) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
} else if (lastAlarm >= FILTERRESULT_ERROR) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}

8
flight/modules/System/systemmod.c
View File

@ -246,10 +246,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
// Turn on the error LED if an alarm is set
#if defined(PIOS_LED_ALARM)
if (AlarmsHasCritical()) {
if (AlarmsHasErrors()) {
PIOS_LED_On(PIOS_LED_ALARM);
} else if ((AlarmsHasErrors() && (cycleCount & 0x1)) ||
(!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
} else if ((AlarmsHasCritical() && (cycleCount & 0x1)) ||
(!AlarmsHasCritical() && AlarmsHasWarnings() && (cycleCount & 0x4))) {
PIOS_LED_On(PIOS_LED_ALARM);
} else {
PIOS_LED_Off(PIOS_LED_ALARM);
@ -429,7 +429,7 @@ static void hwSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
HwSettingsGet(&currentHwSettings);
// check whether the Hw Configuration has changed from the one used at boot time
if (memcmp(&bootHwSettings, &currentHwSettings, sizeof(HwSettingsData)) != 0) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_ERROR, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
}
}

6
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -214,7 +214,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
updateVtolDesiredAttitude(true);
updatePOIBearing();
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
} else {
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
@ -222,7 +222,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
updateVtolDesiredAttitude(false);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
}
}
} else {
@ -247,7 +247,7 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
break;
default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
PathStatusSet(&pathStatus);

3
shared/uavobjectdefinition/systemalarms.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="SystemAlarms" singleinstance="true" settings="false" category="System" priority="true">
<description>Alarms from OpenPilot to indicate failure conditions or warnings. Set by various modules. Some modules may have a module defined Status and Substatus fields that details its condition.</description>
<field name="Alarm" units="" type="enum" options="Uninitialised,OK,Warning,Error,Critical" defaultvalue="Uninitialised">
<field name="Alarm" units="" type="enum" options="Uninitialised,OK,Warning,Critical,Error" defaultvalue="Uninitialised">
<elementnames>
<elementname>SystemConfiguration</elementname>
<elementname>BootFault</elementname>
@ -23,7 +23,6 @@
<elementname>FlightTime</elementname>
<elementname>I2C</elementname>
<elementname>GPS</elementname>
<elementname>Power</elementname>
</elementnames>
</field>
<field name="ExtendedAlarmStatus" units="" type="enum" defaultvalue="None">