mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
some changes...
This commit is contained in:
Corvus Corax
parent
ecc4a529a3
commit
8fe159c457
@ -36,6 +36,8 @@
|
||||
|
||||
// Private constants
|
||||
|
||||
#define STACK_REQUIRED 64
|
||||
|
||||
// simple IAS to TAS aproximation - 2% increase per 1000ft
|
||||
// since we do not have flowing air temperature information
|
||||
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
|
||||
@ -51,10 +53,11 @@ static int32_t init(void);
|
||||
static int32_t filter(stateEstimation *state);
|
||||
|
||||
|
||||
void filterAirInitialize(stateFilter *handle)
|
||||
int32_t filterAirInitialize(stateFilter *handle)
|
||||
{
|
||||
handle->init = &init;
|
||||
handle->filter = &filter;
|
||||
return STACK_REQUIRED;
|
||||
}
|
||||
|
||||
static int32_t init(void)
|
||||
@ -66,12 +69,12 @@ static int32_t init(void)
|
||||
static int32_t filter(stateEstimation *state)
|
||||
{
|
||||
// take static pressure altitude estimation for
|
||||
if (ISSET(state->updated, bar_UPDATED)) {
|
||||
altitude = state->bar[0];
|
||||
if (ISSET(state->updated, SENSORUPDATES_baro)) {
|
||||
altitude = state->baro[0];
|
||||
}
|
||||
// calculate true airspeed estimation
|
||||
if (ISSET(state->updated, air_UPDATED)) {
|
||||
state->air[1] = state->air[0] * IAS2TAS(altitude);
|
||||
if (ISSET(state->updated, SENSORUPDATES_airspeed)) {
|
||||
state->air[1] = state->airspeed[0] * IAS2TAS(altitude);
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
@ -33,25 +33,25 @@
|
||||
#include <openpilot.h>
|
||||
|
||||
typedef enum {
|
||||
gyr_UPDATED = 1 << 0,
|
||||
acc_UPDATED = 1 << 1,
|
||||
mag_UPDATED = 1 << 2,
|
||||
att_UPDATED = 1 << 3,
|
||||
pos_UPDATED = 1 << 4,
|
||||
vel_UPDATED = 1 << 5,
|
||||
air_UPDATED = 1 << 6,
|
||||
bar_UPDATED = 1 << 7,
|
||||
SENSORUPDATES_gyro = 1 << 0,
|
||||
SENSORUPDATES_accel = 1 << 1,
|
||||
SENSORUPDATES_mag = 1 << 2,
|
||||
SENSORUPDATES_attitude = 1 << 3,
|
||||
SENSORUPDATES_pos = 1 << 4,
|
||||
SENSORUPDATES_vel = 1 << 5,
|
||||
SENSORUPDATES_airspeed = 1 << 6,
|
||||
SENSORUPDATES_baro = 1 << 7,
|
||||
} sensorUpdates;
|
||||
|
||||
typedef struct {
|
||||
float gyr[3];
|
||||
float acc[3];
|
||||
float gyro[3];
|
||||
float accel[3];
|
||||
float mag[3];
|
||||
float att[4];
|
||||
float attitude[4];
|
||||
float pos[3];
|
||||
float vel[3];
|
||||
float air[2];
|
||||
float bar[1];
|
||||
float airspeed[2];
|
||||
float baro[1];
|
||||
sensorUpdates updated;
|
||||
} stateEstimation;
|
||||
|
||||
@ -60,20 +60,21 @@ typedef struct {
|
||||
|
||||
|
||||
typedef struct stateFilterStruct {
|
||||
int32_t (*init)(void);
|
||||
int32_t (*filter)(stateEstimation *state);
|
||||
int32_t (*init)(struct stateFilterStruct *self);
|
||||
int32_t (*filter)(struct stateFilterStruct *self, stateEstimation *state);
|
||||
void *localdata;
|
||||
} stateFilter;
|
||||
|
||||
|
||||
void filterMagInitialize(stateFilter *handle);
|
||||
void filterBaroInitialize(stateFilter *handle);
|
||||
void filterAirInitialize(stateFilter *handle);
|
||||
void filterStationaryInitialize(stateFilter *handle);
|
||||
void filterCFInitialize(stateFilter *handle);
|
||||
void filterCFMInitialize(stateFilter *handle);
|
||||
void filterEKF13iInitialize(stateFilter *handle);
|
||||
void filterEKF13Initialize(stateFilter *handle);
|
||||
void filterEKF16iInitialize(stateFilter *handle);
|
||||
void filterEKF16Initialize(stateFilter *handle);
|
||||
int32_t filterMagInitialize(stateFilter *handle);
|
||||
int32_t filterBaroInitialize(stateFilter *handle);
|
||||
int32_t filterAirInitialize(stateFilter *handle);
|
||||
int32_t filterStationaryInitialize(stateFilter *handle);
|
||||
int32_t filterCFInitialize(stateFilter *handle);
|
||||
int32_t filterCFMInitialize(stateFilter *handle);
|
||||
int32_t filterEKF13iInitialize(stateFilter *handle);
|
||||
int32_t filterEKF13Initialize(stateFilter *handle);
|
||||
int32_t filterEKF16iInitialize(stateFilter *handle);
|
||||
int32_t filterEKF16Initialize(stateFilter *handle);
|
||||
|
||||
#endif // STATEESTIMATION_H
|
||||
|
||||
@ -62,12 +62,12 @@
|
||||
|
||||
// Private types
|
||||
|
||||
struct filterQueueStruct;
|
||||
struct filterPipelineStruct;
|
||||
|
||||
typedef struct filterQueueStruct {
|
||||
stateFilter *filter;
|
||||
struct filterQueueStruct *next;
|
||||
} filterQueue;
|
||||
typedef const struct filterPipelineStruct {
|
||||
const stateFilter *filter;
|
||||
const struct filterPipelineStruct *next;
|
||||
} filterPipeline;
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *stateEstimationCallback;
|
||||
@ -76,8 +76,8 @@ static volatile RevoSettingsData revoSettings;
|
||||
static volatile HomeLocationData homeLocation;
|
||||
static float LLA2NEDM[3];
|
||||
static volatile sensorUpdates updatedSensors;
|
||||
static int32_t fusionAlgorithm = -1;
|
||||
static filterQueue *filterChain = NULL;
|
||||
static int32_t fusionAlgorithm = -1;
|
||||
static filterPipeline *filterChain = NULL;
|
||||
|
||||
// different filters available to state estimation
|
||||
static stateFilter magFilter;
|
||||
@ -92,41 +92,41 @@ static stateFilter ekf16Filter;
|
||||
static stateFilter ekf16iFilter;
|
||||
|
||||
// preconfigured filter chains selectable via revoSettings.FusionAlgorithm
|
||||
static const filterQueue *cfQueue = &(filterQueue) {
|
||||
static filterPipeline *cfQueue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &cfFilter,
|
||||
.next = NULL,
|
||||
},
|
||||
}
|
||||
}
|
||||
};
|
||||
static const filterQueue *cfmQueue = &(filterQueue) {
|
||||
static const filterPipeline *cfmQueue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &cfmFilter,
|
||||
.next = NULL,
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
static const filterQueue *ekf13iQueue = &(filterQueue) {
|
||||
static const filterPipeline *ekf13iQueue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &stationaryFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &ekf13iFilter,
|
||||
.next = NULL,
|
||||
}
|
||||
@ -134,28 +134,28 @@ static const filterQueue *ekf13iQueue = &(filterQueue) {
|
||||
}
|
||||
}
|
||||
};
|
||||
static const filterQueue *ekf13Queue = &(filterQueue) {
|
||||
static const filterPipeline *ekf13Queue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &ekf13Filter,
|
||||
.next = NULL,
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
static const filterQueue *ekf16iQueue = &(filterQueue) {
|
||||
static const filterPipeline *ekf16iQueue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &stationaryFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &ekf16iFilter,
|
||||
.next = NULL,
|
||||
}
|
||||
@ -163,13 +163,13 @@ static const filterQueue *ekf16iQueue = &(filterQueue) {
|
||||
}
|
||||
}
|
||||
};
|
||||
static const filterQueue *ekf16Queue = &(filterQueue) {
|
||||
static const filterPipeline *ekf16Queue = &(filterPipeline) {
|
||||
.filter = &magFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &airFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &baroFilter,
|
||||
.next = &(filterQueue) {
|
||||
.next = &(filterPipeline) {
|
||||
.filter = &ekf16Filter,
|
||||
.next = NULL,
|
||||
}
|
||||
@ -185,6 +185,13 @@ static void StateEstimationCb(void);
|
||||
static void getNED(GPSPositionData *gpsPosition, float *NED);
|
||||
static bool sane(float value);
|
||||
|
||||
static inline int32_t maxint32_t(int32_t a, int2_t b)
|
||||
{
|
||||
if (a > b) {
|
||||
return a;
|
||||
}
|
||||
return b;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
@ -221,7 +228,20 @@ int32_t StateEstimationInitialize(void)
|
||||
GPSPositionConnectCallback(&sensorUpdatedCb);
|
||||
GPSVelocityConnectCallback(&sensorUpdatedCb);
|
||||
|
||||
stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE_BYTES);
|
||||
uint32_t stack_required = STACK_SIZE_BYTES;
|
||||
// Initialize Filters
|
||||
stack_required = maxint32_t(stack_required, filterMagInitialize(&magFilter));
|
||||
stack_required = maxint32_t(stack_required, filterBaroInitialize(&baroFilter));
|
||||
stack_required = maxint32_t(stack_required, filterAirInitialize(&airFilter));
|
||||
stack_required = maxint32_t(stack_required, filterStationaryInitialize(&stationaryFilter));
|
||||
stack_required = maxint32_t(stack_required, filterCFInitialize(&cfFilter));
|
||||
stack_required = maxint32_t(stack_required, filterCFMInitialize(&cfmFilter));
|
||||
stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
|
||||
stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
|
||||
stack_required = maxint32_t(stack_required, filterEKF16Initialize(&ekf16Filter));
|
||||
stack_required = maxint32_t(stack_required, filterEKF16iInitialize(&ekf16iFilter));
|
||||
|
||||
stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -237,18 +257,6 @@ int32_t StateEstimationStart(void)
|
||||
// Force settings update to make sure rotation loaded
|
||||
settingsUpdatedCb(NULL);
|
||||
|
||||
// Initialize Filters
|
||||
filterMagInitialize(&magFilter);
|
||||
filterBaroInitialize(&baroFilter);
|
||||
filterAirInitialize(&airFilter);
|
||||
filterStationaryInitialize(&stationaryFilter);
|
||||
filterCFInitialize(&cfFilter);
|
||||
filterCFMInitialize(&cfmFilter);
|
||||
filterEKF13iInitialize(&ekf13iFilter);
|
||||
filterEKF13Initialize(&ekf13Filter);
|
||||
filterEKF16Initialize(&ekf16Filter);
|
||||
filterEKF16iInitialize(&ekf16iFilter);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -259,72 +267,78 @@ MODULE_INITCALL(StateEstimationInitialize, StateEstimationStart)
|
||||
*/
|
||||
static void StateEstimationCb(void)
|
||||
{
|
||||
// alarms flag
|
||||
int8_t alarm = 0;
|
||||
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 filterPipeline *current;
|
||||
static stateEstimation states;
|
||||
|
||||
// set alarm to warning if called through timeout
|
||||
if (updatedSensors == 0) {
|
||||
alarm = 1;
|
||||
}
|
||||
switch (runState) {
|
||||
case RUNSTATE_LOAD:
|
||||
|
||||
// check if a new filter chain should be initialized
|
||||
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
|
||||
FlightStatusData fs;
|
||||
FlightStatusGet(&fs);
|
||||
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
|
||||
const filterQueue *newFilterChain;
|
||||
switch (revoSettings.FusionAlgorithm) {
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
|
||||
newFilterChain = cfQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
|
||||
newFilterChain = cfmQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
|
||||
newFilterChain = ekf13iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
|
||||
newFilterChain = ekf13Queue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR:
|
||||
newFilterChain = ekf16iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR:
|
||||
newFilterChain = ekf16Queue;
|
||||
break;
|
||||
default:
|
||||
newFilterChain = NULL;
|
||||
}
|
||||
// initialize filters in chain
|
||||
filterQueue *current = (filterQueue *)newFilterChain;
|
||||
bool error = 0;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->init();
|
||||
if (result != 0) {
|
||||
error = 1;
|
||||
alarm = 0;
|
||||
|
||||
// set alarm to warning if called through timeout
|
||||
if (updatedSensors == 0) {
|
||||
alarm = 1;
|
||||
}
|
||||
|
||||
// check if a new filter chain should be initialized
|
||||
if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
|
||||
FlightStatusData fs;
|
||||
FlightStatusGet(&fs);
|
||||
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == -1) {
|
||||
const filterPipeline *newFilterChain;
|
||||
switch (revoSettings.FusionAlgorithm) {
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY:
|
||||
newFilterChain = cfQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAG:
|
||||
newFilterChain = cfmQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR:
|
||||
newFilterChain = ekf13iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS13OUTDOOR:
|
||||
newFilterChain = ekf13Queue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS16INDOOR:
|
||||
newFilterChain = ekf16iQueue;
|
||||
break;
|
||||
case REVOSETTINGS_FUSIONALGORITHM_INS16OUTDOOR:
|
||||
newFilterChain = ekf16Queue;
|
||||
break;
|
||||
default:
|
||||
newFilterChain = NULL;
|
||||
}
|
||||
// initialize filters in chain
|
||||
current = (filterPipeline *)newFilterChain;
|
||||
bool error = 0;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->init();
|
||||
if (result != 0) {
|
||||
error = 1;
|
||||
break;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return;
|
||||
} else {
|
||||
// set new fusion algortithm
|
||||
filterChain = (filterPipeline *)newFilterChain;
|
||||
fusionAlgorithm = revoSettings.FusionAlgorithm;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
if (error) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
return;
|
||||
} else {
|
||||
// set new fusion algortithm
|
||||
filterChain = (filterQueue *)newFilterChain;
|
||||
fusionAlgorithm = revoSettings.FusionAlgorithm;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// read updated sensor UAVObjects and set initial state
|
||||
stateEstimation states;
|
||||
states.updated = updatedSensors;
|
||||
updatedSensors ^= states.updated;
|
||||
// read updated sensor UAVObjects and set initial state
|
||||
states.updated = updatedSensors;
|
||||
updatedSensors ^= states.updated;
|
||||
|
||||
// most sensors get only rudimentary sanity checks
|
||||
// most sensors get only rudimentary sanity checks
|
||||
#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \
|
||||
if (ISSET(states.updated, shortname##_UPDATED)) { \
|
||||
sensorname##Data s; \
|
||||
@ -338,10 +352,10 @@ static void StateEstimationCb(void)
|
||||
UNSET(states.updated, shortname##_UPDATED); \
|
||||
} \
|
||||
}
|
||||
SANITYCHECK3(GyroSensor, gyr, x, y, z);
|
||||
SANITYCHECK3(AccelSensor, acc, x, y, z);
|
||||
SANITYCHECK3(MagSensor, mag, x, y, z);
|
||||
SANITYCHECK3(GPSVelocity, vel, North, East, Down);
|
||||
SANITYCHECK3(GyroSensor, gyr, x, y, z);
|
||||
SANITYCHECK3(AccelSensor, acc, x, y, z);
|
||||
SANITYCHECK3(MagSensor, mag, x, y, z);
|
||||
SANITYCHECK3(GPSVelocity, vel, North, East, Down);
|
||||
#define SANITYCHECK1(sensorname, shortname, a1, EXTRACHECK) \
|
||||
if (ISSET(states.updated, shortname##_UPDATED)) { \
|
||||
sensorname##Data s; \
|
||||
@ -353,34 +367,51 @@ static void StateEstimationCb(void)
|
||||
UNSET(states.updated, shortname##_UPDATED); \
|
||||
} \
|
||||
}
|
||||
SANITYCHECK1(BaroSensor, bar, Altitude, 1);
|
||||
SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
|
||||
states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter
|
||||
SANITYCHECK1(BaroSensor, bar, Altitude, 1);
|
||||
SANITYCHECK1(AirspeedSensor, air, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
|
||||
states.air[1] = 0.0f; // sensor does not provide true airspeed, needs to be calculated by filter
|
||||
|
||||
// GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons
|
||||
if (ISSET(states.updated, pos_UPDATED)) {
|
||||
GPSPositionData s;
|
||||
GPSPositionGet(&s);
|
||||
if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) {
|
||||
getNED(&s, states.pos);
|
||||
} else {
|
||||
UNSET(states.updated, pos_UPDATED);
|
||||
// GPS is a tiny bit more tricky as GPSPosition is not float (otherwise the conversion to NED could sit in a filter) but integers, for precision reasons
|
||||
if (ISSET(states.updated, pos_UPDATED)) {
|
||||
GPSPositionData s;
|
||||
GPSPositionGet(&s);
|
||||
if (homeLocation.Set == HOMELOCATION_SET_TRUE && sane(s.Latitude) && sane(s.Longitude) && sane(s.Altitude) && (fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f || fabsf(s.Latitude) > 1e-5f)) {
|
||||
getNED(&s, states.pos);
|
||||
} else {
|
||||
UNSET(states.updated, pos_UPDATED);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// at this point sensor state is stored in "states" with some rudimentary filtering applied
|
||||
// at this point sensor state is stored in "states" with some rudimentary filtering applied
|
||||
|
||||
// apply all filters in the current filter chain
|
||||
filterQueue *current = (filterQueue *)filterChain;
|
||||
while (current != NULL) {
|
||||
int32_t result = current->filter->filter(&states);
|
||||
if (result > alarm) {
|
||||
alarm = result;
|
||||
// apply all filters in the current filter chain
|
||||
current = (filterPipeline *)filterChain;
|
||||
|
||||
// we are not done, re-dispatch self execution
|
||||
runState = RUNSTATE_FILTER;
|
||||
DelayedCallbackDispatch(stateEstimationCallback);
|
||||
break;
|
||||
|
||||
case RUNSTATE_FILTER:
|
||||
|
||||
if (current != NULL) {
|
||||
int32_t result = current->filter->filter(&states);
|
||||
if (result > alarm) {
|
||||
alarm = result;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
// the final output of filters is saved in state variables
|
||||
// we are not done, re-dispatch self execution
|
||||
if (!current) {
|
||||
runState = RUNSTATE_SAVE;
|
||||
}
|
||||
DelayedCallbackDispatch(stateEstimationCallback);
|
||||
break;
|
||||
|
||||
case RUNSTATE_SAVE:
|
||||
|
||||
// the final output of filters is saved in state variables
|
||||
#define STORE3(statename, shortname, a1, a2, a3) \
|
||||
if (ISSET(states.updated, shortname##_UPDATED)) { \
|
||||
statename##Data s; \
|
||||
@ -390,11 +421,11 @@ static void StateEstimationCb(void)
|
||||
s.a3 = states.shortname[2]; \
|
||||
statename##Set(&s); \
|
||||
}
|
||||
STORE3(GyroState, gyr, x, y, z);
|
||||
STORE3(AccelState, acc, x, y, z);
|
||||
STORE3(MagState, mag, x, y, z);
|
||||
STORE3(PositionState, pos, North, East, Down);
|
||||
STORE3(VelocityState, vel, North, East, Down);
|
||||
STORE3(GyroState, gyr, x, y, z);
|
||||
STORE3(AccelState, acc, x, y, z);
|
||||
STORE3(MagState, mag, x, y, z);
|
||||
STORE3(PositionState, pos, North, East, Down);
|
||||
STORE3(VelocityState, vel, North, East, Down);
|
||||
#define STORE2(statename, shortname, a1, a2) \
|
||||
if (ISSET(states.updated, shortname##_UPDATED)) { \
|
||||
statename##Data s; \
|
||||
@ -403,7 +434,7 @@ static void StateEstimationCb(void)
|
||||
s.a2 = states.shortname[1]; \
|
||||
statename##Set(&s); \
|
||||
}
|
||||
STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed);
|
||||
STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed);
|
||||
#define STORE1(statename, shortname, a1) \
|
||||
if (ISSET(states.updated, shortname##_UPDATED)) { \
|
||||
statename##Data s; \
|
||||
@ -411,45 +442,54 @@ static void StateEstimationCb(void)
|
||||
s.a1 = states.shortname[0]; \
|
||||
statename##Set(&s); \
|
||||
}
|
||||
STORE1(BaroState, bar, Altitude);
|
||||
// attitude nees manual conversion from quaternion to euler
|
||||
if (ISSET(states.updated, att_UPDATED)) { \
|
||||
AttitudeStateData s;
|
||||
AttitudeStateGet(&s);
|
||||
s.q1 = states.att[0];
|
||||
s.q2 = states.att[1];
|
||||
s.q3 = states.att[2];
|
||||
s.q4 = states.att[3];
|
||||
Quaternion2RPY(&s.q1, &s.Roll);
|
||||
AttitudeStateSet(&s);
|
||||
}
|
||||
STORE1(BaroState, bar, Altitude);
|
||||
// attitude nees manual conversion from quaternion to euler
|
||||
if (ISSET(states.updated, att_UPDATED)) { \
|
||||
AttitudeStateData s;
|
||||
AttitudeStateGet(&s);
|
||||
s.q1 = states.att[0];
|
||||
s.q2 = states.att[1];
|
||||
s.q3 = states.att[2];
|
||||
s.q4 = states.att[3];
|
||||
Quaternion2RPY(&s.q1, &s.Roll);
|
||||
AttitudeStateSet(&s);
|
||||
}
|
||||
|
||||
// throttle alarms, raise alarm flags immediately
|
||||
// but require system to run for a while before decreasing
|
||||
// to prevent alarm flapping
|
||||
if (alarm >= lastAlarm) {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
} else {
|
||||
if (alarmcounter < 100) {
|
||||
alarmcounter++;
|
||||
} else {
|
||||
// throttle alarms, raise alarm flags immediately
|
||||
// but require system to run for a while before decreasing
|
||||
// to prevent alarm flapping
|
||||
if (alarm >= lastAlarm) {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
} else {
|
||||
if (alarmcounter < 100) {
|
||||
alarmcounter++;
|
||||
} else {
|
||||
lastAlarm = alarm;
|
||||
alarmcounter = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// clear alarms if everything is alright, then schedule callback execution after timeout
|
||||
if (lastAlarm == 1) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else if (lastAlarm == 2) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else if (lastAlarm >= 3) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
// clear alarms if everything is alright, then schedule callback execution after timeout
|
||||
if (lastAlarm == 1) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING);
|
||||
} else if (lastAlarm == 2) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else if (lastAlarm >= 3) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
} else {
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
|
||||
}
|
||||
|
||||
// we are done, re-schedule next self execution
|
||||
runState = RUNSTATE_LOAD;
|
||||
if (updatedSensors) {
|
||||
DelayedCallbackDispatch(stateEstimationCallback);
|
||||
} else {
|
||||
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
break;
|
||||
}
|
||||
DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user