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

Merge remote-tracking branch 'origin/Bertrand/OP-1308_Set_Same_Logic_To_Alarms' into next

Browse Source 
Conflicts:
	flight/libraries/sanitycheck.c
	flight/modules/StateEstimation/filtermag.c
	flight/modules/System/systemmod.c
This commit is contained in:
Alessio Morale 2014-06-26 20:31:27 +02:00
commit 1b823b7b92
30 changed files with 222 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/libraries/inc/insgps.h
View File

@ -67,7 +67,6 @@ void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetGyroBiasVar(float gyro_bias_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetG(float g_e);
void INSSetMagVar(float scaled_mag_var[3]);
void INSSetBaroVar(float baro_var);
void INSPosVelReset(float pos[3], float vel[3]);

9
flight/libraries/insgps13state.c
View File

@ -91,8 +91,6 @@ static struct EKFData {
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
// local gravity vector
float g_e;
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
@ -288,11 +286,6 @@ void INSSetMagNorth(float B[3])
ekf.Be[2] = B[2] / mag;
}
void INSSetG(float g_e)
{
ekf.g_e = g_e;
}
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
@ -648,7 +641,7 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
az;
Xdot[5] =
2.0f * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + ekf.g_e;
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;

2
flight/libraries/sanitycheck.c
View File

@ -42,7 +42,7 @@
#include <taskinfo.h>
// a number of useful macros
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_ERROR))
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
/****************************

3
flight/modules/Airspeed/airspeed.c
View File

@ -45,6 +45,7 @@
#include "baro_airspeed_etasv3.h"
#include "baro_airspeed_mpxv.h"
#include "gps_airspeed.h"
#include "airspeedalarm.h"
#include "taskinfo.h"
// Private constants
@ -153,7 +154,7 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
// if sensor type changed reset Airspeed alarm
if (airspeedSettings.AirspeedSensorType != lastAirspeedSensorType) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_DEFAULT);
AirspeedAlarm(SYSTEMALARMS_ALARM_DEFAULT);
lastAirspeedSensorType = airspeedSettings.AirspeedSensorType;
switch (airspeedSettings.AirspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_NONE:

65
flight/modules/Airspeed/airspeedalarm.c Normal file
View File

@ -0,0 +1,65 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file airspeedalarm.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
* Output object: none
*
* Handle locally airspeed alarms issue changes to PIOS only when necessary
*
*/
#include "airspeedalarm.h"
// local variable
static SystemAlarmsAlarmOptions severitySet = SYSTEMALARMS_ALARM_UNINITIALISED;
// functions
/**
* Handle airspeed alarms and isuue an Alarm to PIOS only if necessary
*/
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity)
{
if (severity == severitySet) {
return false;
}
severitySet = severity;
return AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, severity) == 0;
}
/**
* @}
* @}
*/

7
flight/modules/Airspeed/baro_airspeed_etasv3.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_ETASV3)
@ -64,13 +65,13 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -95,7 +96,7 @@ void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettings
// Compute airspeed
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}

9
flight/modules/Airspeed/baro_airspeed_mpxv.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#if defined(PIOS_INCLUDE_MPXV)
@ -63,7 +64,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
// Ensure that the ADC pin is properly configured
if (airspeedADCPin < 0) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
@ -76,7 +77,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
}
@ -84,7 +85,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
// Calibrate sensor by averaging zero point value
if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { // First let sensor warm up and stabilize.
calibrationCount++;
@ -109,7 +110,7 @@ void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsDa
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * (alpha) + airspeedSensor->CalibratedAirspeed * (1.0f - alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MPXV) */

9
flight/modules/Airspeed/baro_airspeed_ms4525do.c
View File

@ -40,6 +40,7 @@
#include "hwsettings.h"
#include "airspeedsettings.h"
#include "airspeedsensor.h" // object that will be updated by the module
#include "airspeedalarm.h"
#include "taskinfo.h"
#if defined(PIOS_INCLUDE_MS4525DO)
@ -74,7 +75,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
airspeedSensor->SensorValueTemperature = -1;
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return;
}
@ -87,7 +88,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
filter_reg = (airspeedSensor->SensorValue << FILTER_SHIFT);
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
calibrationCount++;
@ -101,7 +102,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
}
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_WARNING);
return;
}
}
@ -128,7 +129,7 @@ void baro_airspeedGetMS4525DO(AirspeedSensorData *airspeedSensor, AirspeedSettin
airspeedSensor->TrueAirspeed = airspeedSensor->CalibratedAirspeed * TASFACTOR * sqrtf(T);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
// everything is fine so set ALARM_OK
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
#endif /* if defined(PIOS_INCLUDE_MS4525DO) */

29
flight/modules/Airspeed/gps_airspeed.c
View File

@ -37,6 +37,7 @@
#include "airspeedsettings.h"
#include "gps_airspeed.h"
#include "CoordinateConversions.h"
#include "airspeedalarm.h"
#include <pios_math.h>
@ -45,7 +46,6 @@
#define GPS_AIRSPEED_BIAS_KI 0.1f // Needs to be settable in a UAVO
#define SAMPLING_DELAY_MS_GPS 100 // Needs to be settable in a UAVO
#define GPS_AIRSPEED_TIME_CONSTANT_MS 500.0f // Needs to be settable in a UAVO
#define COSINE_OF_5_DEG 0.9961947f
// Private types
struct GPSGlobals {
@ -60,12 +60,6 @@ struct GPSGlobals {
static struct GPSGlobals *gps;
// Private functions
// a simple square inline function based on multiplication faster than powf(x,2.0f)
static inline float Sq(float x)
{
return x * x;
}
/*
* Initialize function loads first data sets, and allocates memory for structure.
@ -109,11 +103,6 @@ void gps_airspeedInitialize()
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
/* Remark regarding "IMU Wind Estimation": The approach includes errors when |V| is
* not constant, i.e. when the change in V_gps does not solely come from a reorientation
* this error depends strongly on the time scale considered. Is the time between t1 and t2 too
* small, "spikes" absorving unconsidred acceleration will arise
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
@ -132,39 +121,35 @@ void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *air
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < COSINE_OF_5_DEG) {
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = Sq(gpsVelData.North - gps->gpsVelOld_N) + Sq(gpsVelData.East - gps->gpsVelOld_E) + Sq(gpsVelData.Down - gps->gpsVelOld_D);
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = Sq(Rbe[0][0] - gps->RbeCol1_old[0]) + Sq(Rbe[0][1] - gps->RbeCol1_old[1]) + Sq(Rbe[0][2] - gps->RbeCol1_old[2]);
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
} else if (!IS_REAL(airspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_WARNING);
AirspeedAlarm(SYSTEMALARMS_ALARM_ERROR);
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
AirspeedAlarm(SYSTEMALARMS_ALARM_OK);
}
// Save old variables for next pass

45
flight/modules/Airspeed/inc/airspeedalarm.h Normal file
View File

@ -0,0 +1,45 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup AirspeedModule Airspeed Module
* @brief Handle locally airspeed alarms issue changes to PIOS only when necessary
* @{
*
* @file airspeedalarm.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Airspeed module, reads temperature and pressure from MS4525DO
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef AIRSPEEDALARM_H
#define AIRSPEEDALARM_H
#include <openpilot.h>
#include "alarms.h"
bool AirspeedAlarm(SystemAlarmsAlarmOptions severity);
#endif // AIRSPEEDALARM_H
/**
* @}
* @}
*/

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

@ -193,7 +193,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;
@ -221,7 +221,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

@ -173,7 +173,7 @@ static void pathPlannerTask()
// copy pasta: same calculation as in manualcontrol, set return to home coordinates
plan_setup_positionHold();
}
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
return;
}

12
flight/modules/Stabilization/innerloop.c
View File

@ -119,21 +119,21 @@ static void stabilizationInnerloopTask()
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// error if rate loop skipped more than 4 executions
error = true;
// critical if rate loop skipped more than 4 executions
crit = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// critical if gyro didn't update at all!
crit = true;
// error if gyro didn't update at all!
error = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// error if we missed 3 gyro updates
error = true;
// critical if we missed 3 gyro updates
crit = true;
}
stabSettings.monitor.gyroupdates = 0;

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

@ -31,6 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>

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

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

@ -74,7 +74,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 settingsUpdatedCb(UAVObjEvent *ev);
@ -114,7 +114,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;
@ -204,7 +204,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
}
return 0;
return FILTERRESULT_OK;
}
void settingsUpdatedCb(UAVObjEvent *ev)

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

30
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;
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;
@ -345,7 +345,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
@ -356,7 +356,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);
@ -449,13 +449,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
}
}

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

@ -83,7 +83,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);
@ -193,7 +193,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;
@ -222,7 +222,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);
@ -316,11 +316,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]) };
@ -361,7 +361,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
}
INSSetMagNorth(this->homeLocation.Be);
INSSetG(this->homeLocation.g_e);
if (!this->usePos) {
// position and velocity variance used in indoor mode
@ -433,9 +432,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

@ -59,7 +59,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 checkMagValidity(struct data *this, float mag[3]);
static void magOffsetEstimation(struct data *this, float mag[3]);
@ -87,7 +87,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;
@ -98,7 +98,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;

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

@ -224,6 +224,7 @@ static const filterPipeline *ekf13iQueue = &(filterPipeline) {
}
}
};
static const filterPipeline *ekf13Queue = &(filterPipeline) {
.filter = &magFilter,
.next = &(filterPipeline) {
@ -334,9 +335,9 @@ 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 uint16_t alarmcounter = 0;
static filterResult alarm = FILTERRESULT_OK;
static filterResult lastAlarm = FILTERRESULT_UNINITIALISED;
static uint16_t alarmcounter = 0;
static const filterPipeline *current;
static stateEstimation states;
static uint32_t last_time;
@ -352,12 +353,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();
@ -442,7 +443,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;
}
@ -498,12 +499,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);
}

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

@ -403,7 +403,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);
}
}
@ -607,7 +607,7 @@ static void updateSystemAlarms()
UAVObjClearStats();
EventClearStats();
if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) {
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_ERROR);
AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM);
}

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

@ -139,7 +139,6 @@ int32_t VtolPathFollowerInitialize()
AccessoryDesiredInitialize();
PoiLearnSettingsInitialize();
PoiLocationInitialize();
HomeLocationInitialize();
vtolpathfollower_enabled = true;
} else {
vtolpathfollower_enabled = false;
@ -159,7 +158,6 @@ static float eastPosIntegral = 0;
static float downPosIntegral = 0;
static float thrustOffset = 0;
static float gravity;
/**
* Module thread, should not return.
*/
@ -183,7 +181,6 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
HomeLocationg_eGet(&gravity);
SystemSettingsGet(&systemSettings);
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
@ -216,7 +213,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) {
@ -224,7 +221,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 {
@ -249,7 +246,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);
@ -686,7 +683,7 @@ static void updateNedAccel()
accel_ned[i] += Rbe[j][i] * accel[j];
}
}
accel_ned[2] += gravity;
accel_ned[2] += 9.81f;
NedAccelData accelData;
NedAccelGet(&accelData);

4
flight/targets/boards/coptercontrol/firmware/inc/pios_config.h
View File

@ -171,10 +171,10 @@
#endif
#define PIOS_TELEM_RX_STACK_SIZE 410
#define PIOS_TELEM_TX_STACK_SIZE 560
#define PIOS_EVENTDISPATCHER_STACK_SIZE 95
#define PIOS_EVENTDISPATCHER_STACK_SIZE 95
/* This can't be too high to stop eventdispatcher thread overflowing */
#define PIOS_EVENTDISAPTCHER_QUEUE 10
#define PIOS_EVENTDISAPTCHER_QUEUE 10
/* Revolution series */
/* #define REVOLUTION */

2
ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp
View File

@ -86,7 +86,7 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent)
HwSettings *hwSettings = HwSettings::GetInstance(getObjectManager());
HwSettings::DataFields hwSettingsData = hwSettings->getData();
if (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) {
if(hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_GPS] != HwSettings::OPTIONALMODULES_ENABLED) {
m_telemetry->gpsProtocol->setEnabled(false);
m_telemetry->gpsProtocol->setToolTip(tr("Enable GPS module and reboot the board to be able to select GPS protocol"));
} else {

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>
@ -24,7 +24,6 @@
<elementname>FlightTime</elementname>
<elementname>I2C</elementname>
<elementname>GPS</elementname>
<elementname>Power</elementname>
</elementnames>
</field>
<field name="ExtendedAlarmStatus" units="" type="enum" defaultvalue="None">