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

some small bugfixes to filterekf

Browse Source
This commit is contained in:
Corvus Corax 2013-05-25 00:05:06 +02:00
parent b916df5448
commit 902bf29c92
1 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,14 @@
#define STACK_REQUIRED 2048
#define IMPORT_SENSOR_IF_UPDATED(shortname, num) \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \
for (t = 0; t < num; t++) { \
this->work.shortname[t] = state->shortname[t]; \
} \
}
// Private types
struct data {
EKFConfigurationData ekfConfiguration;
@ -191,20 +199,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->work.updated |= state->updated;
// Get most recent data
#define UPDATE(shortname, num) \
if (IS_SET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \
for (t = 0; t < num; t++) { \
this->work.shortname[t] = state->shortname[t]; \
} \
}
UPDATE(gyro, 3);
UPDATE(accel, 3);
UPDATE(mag, 3);
UPDATE(baro, 1);
UPDATE(pos, 3);
UPDATE(vel, 3);
UPDATE(airspeed, 2);
IMPORT_SENSOR_IF_UPDATED(gyro, 3);
IMPORT_SENSOR_IF_UPDATED(accel, 3);
IMPORT_SENSOR_IF_UPDATED(mag, 3);
IMPORT_SENSOR_IF_UPDATED(baro, 1);
IMPORT_SENSOR_IF_UPDATED(pos, 3);
IMPORT_SENSOR_IF_UPDATED(vel, 3);
IMPORT_SENSOR_IF_UPDATED(airspeed, 2);
// check whether mandatory updates are present accels must have been supplied already,
// and gyros must be supplied just now for a prediction step to take place
@ -332,9 +333,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] += Nav.gyro_bias[0];
state->gyro[1] += Nav.gyro_bias[1];
state->gyro[2] += Nav.gyro_bias[2];
state->gyro[0] -= Nav.gyro_bias[0];
state->gyro[1] -= Nav.gyro_bias[1];
state->gyro[2] -= Nav.gyro_bias[2];
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];