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

filterchain rework

Browse Source
This commit is contained in:
Corvus Corax 2013-05-22 21:11:48 +02:00
parent 8fe159c457
commit b56de3b66b
12 changed files with 450 additions and 424 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -43,38 +43,43 @@
#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f))
// Private types
// Private variables
static float altitude = 0.0f;
struct data {
float altitude;
};
// Private functions
static int32_t init(void);
static int32_t filter(stateEstimation *state);
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
int32_t filterAirInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(void)
static int32_t init(stateFilter *self)
{
altitude = 0.0f;
struct data *this = (struct data *)self->localdata;
this->altitude = 0.0f;
return 0;
}
static int32_t filter(stateEstimation *state)
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
// take static pressure altitude estimation for
if (ISSET(state->updated, SENSORUPDATES_baro)) {
altitude = state->baro[0];
this->altitude = state->baro[0];
}
// calculate true airspeed estimation
if (ISSET(state->updated, SENSORUPDATES_airspeed)) {
state->air[1] = state->airspeed[0] * IAS2TAS(altitude);
state->airspeed[1] = state->airspeed[0] * IAS2TAS(this->altitude);
}
return 0;

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

@ -34,6 +34,8 @@
// Private constants
#define STACK_REQUIRED 64
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
@ -41,49 +43,57 @@
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// Private types
struct data {
float baroOffset;
bool first_run;
};
// Private variables
static float baroOffset = 0.0f;
static bool first_run = 1;
// Private functions
static int32_t init(void);
static int32_t filter(stateEstimation *state);
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
void filterBaroInitialize(stateFilter *handle)
int32_t filterBaroInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(void)
static int32_t init(stateFilter *self)
{
baroOffset = 0.0f;
first_run = 1;
struct data *this = (struct data *)self->localdata;
this->baroOffset = 0.0f;
this->first_run = 1;
return 0;
}
static int32_t filter(stateEstimation *state)
static int32_t filter(stateFilter *self, stateEstimation *state)
{
if (first_run) {
struct data *this = (struct data *)self->localdata;
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (ISSET(state->updated, bar_UPDATED)) {
first_run = 0;
baroOffset = state->bar[0];
if (ISSET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->baroOffset = state->baro[0];
}
} else {
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (ISSET(state->updated, pos_UPDATED)) {
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
if (ISSET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (-state->pos[2] - state->bar[0]);
* (-state->pos[2] - state->baro[0]);
}
// calculate bias corrected altitude
if (ISSET(state->updated, bar_UPDATED)) {
state->bar[0] -= baroOffset;
if (ISSET(state->updated, SENSORUPDATES_baro)) {
state->baro[0] -= this->baroOffset;
}
}

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

@ -44,34 +44,36 @@
// Private constants
#define STACK_REQUIRED 512
// Private types
struct data {
AttitudeSettingsData attitudeSettings;
HomeLocationData homeLocation;
bool first_run;
bool useMag;
float currentAccel[3];
float currentMag[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
float accel_alpha;
bool accel_filter_enabled;
int32_t timeval;
uint8_t init;
};
// Private variables
static AttitudeSettingsData attitudeSettings;
bool initialized = 0;
static FlightStatusData flightStatus;
static HomeLocationData homeLocation;
static bool initialized = 0;
static bool first_run = 1;
static bool useMag = 0;
static float currentAccel[3];
static float currentMag[3];
static float gyroBias[3];
static bool accelUpdated = 0;
static bool magUpdated = 0;
static float accel_alpha = 0;
static bool accel_filter_enabled = false;
// Private functions
static int32_t initwithmag(void);
static int32_t initwithoutmag(void);
static int32_t maininit(void);
static int32_t filter(stateEstimation *state);
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4]);
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 void flightStatusUpdatedCb(UAVObjEvent *ev);
@ -87,52 +89,62 @@ static void globalInit(void)
}
}
void filterCFInitialize(stateFilter *handle)
int32_t filterCFInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithoutmag;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
void filterCFMInitialize(stateFilter *handle)
int32_t filterCFMInitialize(stateFilter *handle)
{
globalInit();
handle->init = &initwithmag;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t initwithmag(void)
static int32_t initwithmag(stateFilter *self)
{
useMag = 1;
return maininit();
struct data *this = (struct data *)self->localdata;
this->useMag = 1;
return maininit(self);
}
static int32_t initwithoutmag(void)
static int32_t initwithoutmag(stateFilter *self)
{
useMag = 0;
return maininit();
struct data *this = (struct data *)self->localdata;
this->useMag = 0;
return maininit(self);
}
static int32_t maininit(void)
static int32_t maininit(stateFilter *self)
{
first_run = 1;
accelUpdated = 0;
AttitudeSettingsGet(&attitudeSettings);
HomeLocationGet(&homeLocation);
struct data *this = (struct data *)self->localdata;
this->first_run = 1;
this->accelUpdated = 0;
AttitudeSettingsGet(&this->attitudeSettings);
HomeLocationGet(&this->homeLocation);
const float fakeDt = 0.0025f;
if (attitudeSettings.AccelTau < 0.0001f) {
accel_alpha = 0; // not trusting this to resolve to 0
accel_filter_enabled = false;
if (this->attitudeSettings.AccelTau < 0.0001f) {
this->accel_alpha = 0; // not trusting this to resolve to 0
this->accel_filter_enabled = false;
} else {
accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau);
accel_filter_enabled = true;
this->accel_alpha = expf(-fakeDt / this->attitudeSettings.AccelTau);
this->accel_filter_enabled = true;
}
// reset gyro Bias
gyroBias[0] = 0.0f;
gyroBias[1] = 0.0f;
gyroBias[2] = 0.0f;
this->gyroBias[0] = 0.0f;
this->gyroBias[1] = 0.0f;
this->gyroBias[2] = 0.0f;
return 0;
}
@ -140,47 +152,49 @@ static int32_t maininit(void)
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateEstimation *state)
static int32_t filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
int32_t result = 0;
if (ISSET(state->updated, mag_UPDATED)) {
magUpdated = 1;
currentMag[0] = state->mag[0];
currentMag[1] = state->mag[1];
currentMag[2] = state->mag[2];
if (ISSET(state->updated, SENSORUPDATES_mag)) {
this->magUpdated = 1;
this->currentMag[0] = state->mag[0];
this->currentMag[1] = state->mag[1];
this->currentMag[2] = state->mag[2];
}
if (ISSET(state->updated, acc_UPDATED)) {
accelUpdated = 1;
currentAccel[0] = state->acc[0];
currentAccel[1] = state->acc[1];
currentAccel[2] = state->acc[2];
if (ISSET(state->updated, SENSORUPDATES_accel)) {
this->accelUpdated = 1;
this->currentAccel[0] = state->accel[0];
this->currentAccel[1] = state->accel[1];
this->currentAccel[2] = state->accel[2];
}
if (ISSET(state->updated, gyr_UPDATED)) {
if (accelUpdated) {
float att[4];
result = complementaryFilter(state->gyr, currentAccel, currentMag, att);
if (ISSET(state->updated, SENSORUPDATES_gyro)) {
if (this->accelUpdated) {
float attitude[4];
result = complementaryFilter(this, state->gyro, this->currentAccel, this->currentMag, attitude);
if (!result) {
state->att[0] = att[0];
state->att[1] = att[1];
state->att[2] = att[2];
state->att[3] = att[3];
state->updated |= att_UPDATED;
state->attitude[0] = attitude[0];
state->attitude[1] = attitude[1];
state->attitude[2] = attitude[2];
state->attitude[3] = attitude[3];
state->updated |= SENSORUPDATES_attitude;
}
accelUpdated = 0;
magUpdated = 0;
this->accelUpdated = 0;
this->magUpdated = 0;
}
}
return result;
}
static inline void apply_accel_filter(const float *raw, float *filtered)
static inline void apply_accel_filter(const struct data *this, const float *raw, float *filtered)
{
if (accel_filter_enabled) {
filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha);
filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha);
filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha);
if (this->accel_filter_enabled) {
filtered[0] = filtered[0] * this->accel_alpha + raw[0] * (1 - this->accel_alpha);
filtered[1] = filtered[1] * this->accel_alpha + raw[1] * (1 - this->accel_alpha);
filtered[2] = filtered[2] * this->accel_alpha + raw[2] * (1 - this->accel_alpha);
} else {
filtered[0] = raw[0];
filtered[1] = raw[1];
@ -188,19 +202,17 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
}
}
static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3], float q[4])
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
{
static int32_t timeval;
float dT;
static uint8_t init = 0;
float magKp = 0.0f; // TODO: make this non hardcoded at some point
float magKi = 0.000001f;
// During initialization and
if (first_run) {
if (this->first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// wait until mags have been updated
if (!magUpdated) {
if (!this->magUpdated) {
return 1;
}
#else
@ -210,7 +222,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
#endif
AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState);
init = 0;
this->init = 0;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
@ -235,37 +247,37 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, q);
RPY2Quaternion(&attitudeState.Roll, attitude);
first_run = 0;
this->first_run = 0;
timeval = PIOS_DELAY_GetRaw();
this->timeval = PIOS_DELAY_GetRaw();
return 0;
}
if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.9f;
this->attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.9f;
attitudeSettings.YawBiasRate = 0.23f;
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.9f;
this->attitudeSettings.YawBiasRate = 0.23f;
magKp = 1.0f;
init = 0;
} else if (init == 0) {
this->init = 0;
} else if (this->init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
AttitudeSettingsGet(&this->attitudeSettings);
magKp = 0.01f;
init = 1;
this->init = 1;
}
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
dT = PIOS_DELAY_DiffuS(this->timeval) / 1000000.0f;
this->timeval = PIOS_DELAY_GetRaw();
if (dT < 0.001f) { // safe bounds
dT = 0.001f;
}
@ -274,21 +286,21 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
AttitudeStateGet(&attitudeState);
// Get the current attitude estimate
quat_copy(&attitudeState.q1, q);
quat_copy(&attitudeState.q1, attitude);
float accels_filtered[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(accel, accels_filtered);
apply_accel_filter(this, accel, accels_filtered);
// Rotate gravity to body frame and cross with accels
float grot[3];
grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
grot[0] = -(2.0f * (attitude[1] * attitude[3] - attitude[0] * attitude[2]));
grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
float grot_filtered[3];
float accel_err[3];
apply_accel_filter(grot, grot_filtered);
apply_accel_filter(this, grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
// Account for accel magnitude
@ -299,7 +311,7 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
// Account for filtered gravity vector magnitude
float grot_mag;
if (accel_filter_enabled) {
if (this->accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
} else {
grot_mag = 1.0f;
@ -313,14 +325,14 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
accel_err[2] /= (accel_mag * grot_mag);
float mag_err[3] = { 0.0f };
if (magUpdated && useMag) {
if (this->magUpdated && this->useMag) {
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
Quaternion2R(q, Rbe);
Quaternion2R(attitude, Rbe);
rot_mult(Rbe, homeLocation.Be, brot);
rot_mult(Rbe, this->homeLocation.Be, brot);
float mag_len = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] /= mag_len;
@ -343,57 +355,57 @@ static int32_t complementaryFilter(float gyro[3], float accel[3], float mag[3],
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyroBias[0] -= accel_err[0] * attitudeSettings.AccelKi;
gyroBias[1] -= accel_err[1] * attitudeSettings.AccelKi;
gyroBias[2] -= mag_err[2] * magKi;
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi;
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi;
this->gyroBias[2] -= mag_err[2] * magKi;
// Correct rates based on integral coefficient
gyro[0] -= gyroBias[0];
gyro[1] -= gyroBias[1];
gyro[2] -= gyroBias[2];
gyro[0] -= this->gyroBias[0];
gyro[1] -= this->gyroBias[1];
gyro[2] -= this->gyroBias[2];
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
// Correct rates based on proportional coefficient
gyrotmp[0] += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrotmp[1] += accel_err[1] * attitudeSettings.AccelKp / dT;
if (useMag) {
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
if (this->useMag) {
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
} else {
gyrotmp[2] += accel_err[2] * attitudeSettings.AccelKp / dT;
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
}
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = DEG2RAD(-q[1] * gyrotmp[0] - q[2] * gyrotmp[1] - q[3] * gyrotmp[2]) * dT / 2;
qdot[1] = DEG2RAD(q[0] * gyrotmp[0] - q[3] * gyrotmp[1] + q[2] * gyrotmp[2]) * dT / 2;
qdot[2] = DEG2RAD(q[3] * gyrotmp[0] + q[0] * gyrotmp[1] - q[1] * gyrotmp[2]) * dT / 2;
qdot[3] = DEG2RAD(-q[2] * gyrotmp[0] + q[1] * gyrotmp[1] + q[0] * gyrotmp[2]) * dT / 2;
qdot[0] = DEG2RAD(-attitude[1] * gyrotmp[0] - attitude[2] * gyrotmp[1] - attitude[3] * gyrotmp[2]) * dT / 2;
qdot[1] = DEG2RAD(attitude[0] * gyrotmp[0] - attitude[3] * gyrotmp[1] + attitude[2] * gyrotmp[2]) * dT / 2;
qdot[2] = DEG2RAD(attitude[3] * gyrotmp[0] + attitude[0] * gyrotmp[1] - attitude[1] * gyrotmp[2]) * dT / 2;
qdot[3] = DEG2RAD(-attitude[2] * gyrotmp[0] + attitude[1] * gyrotmp[1] + attitude[0] * gyrotmp[2]) * dT / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
attitude[0] = attitude[0] + qdot[0];
attitude[1] = attitude[1] + qdot[1];
attitude[2] = attitude[2] + qdot[2];
attitude[3] = attitude[3] + qdot[3];
if (q[0] < 0.0f) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
if (attitude[0] < 0.0f) {
attitude[0] = -attitude[0];
attitude[1] = -attitude[1];
attitude[2] = -attitude[2];
attitude[3] = -attitude[3];
}
// Renomalize
float qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
float qmag = sqrtf(attitude[0] * attitude[0] + attitude[1] * attitude[1] + attitude[2] * attitude[2] + attitude[3] * attitude[3]);
attitude[0] = attitude[0] / qmag;
attitude[1] = attitude[1] / qmag;
attitude[2] = attitude[2] / qmag;
attitude[3] = attitude[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) {
first_run = 1;
this->first_run = 1;
return 2;
}

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

@ -44,25 +44,33 @@
// Private constants
#define STACK_REQUIRED 2048
// Private types
struct data {
EKFConfigurationData ekfConfiguration;
HomeLocationData homeLocation;
bool usePos;
int32_t init_stage;
stateEstimation work;
uint32_t ins_last_time;
bool inited;
};
// Private variables
static EKFConfigurationData ekfConfiguration;
static HomeLocationData homeLocation;
static bool initialized = 0;
static bool first_run = 1;
static bool usePos = 0;
// Private functions
static int32_t init13i(void);
static int32_t init13(void);
static int32_t maininit(void);
static int32_t filter(stateEstimation *state);
static inline bool invalid(float data);
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 inline bool invalid_var(float data);
static void globalInit(void);
@ -77,73 +85,90 @@ static void globalInit(void)
}
}
void filterEKF13iInitialize(stateFilter *handle)
int32_t filterEKF13iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
void filterEKF13Initialize(stateFilter *handle)
int32_t filterEKF13Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
// XXX
// TODO: Until the 16 state EKF is implemented, run 13 state, so compilation runs through
// XXX
void filterEKF16iInitialize(stateFilter *handle)
int32_t filterEKF16iInitialize(stateFilter *handle)
{
globalInit();
handle->init = &init13i;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
void filterEKF16Initialize(stateFilter *handle)
int32_t filterEKF16Initialize(stateFilter *handle)
{
globalInit();
handle->init = &init13;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init13i(void)
static int32_t init13i(stateFilter *self)
{
usePos = 0;
return maininit();
struct data *this = (struct data *)self->localdata;
this->usePos = 0;
return maininit(self);
}
static int32_t init13(void)
static int32_t init13(stateFilter *self)
{
usePos = 1;
return maininit();
struct data *this = (struct data *)self->localdata;
this->usePos = 1;
return maininit(self);
}
static int32_t maininit(void)
static int32_t maininit(stateFilter *self)
{
first_run = 1;
struct data *this = (struct data *)self->localdata;
EKFConfigurationGet(&ekfConfiguration);
this->inited = false;
this->init_stage = 0;
this->work.updated = 0;
this->ins_last_time = PIOS_DELAY_GetRaw();
EKFConfigurationGet(&this->ekfConfiguration);
int t;
// plausibility check
for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.P[t])) {
if (invalid_var(this->ekfConfiguration.P[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.Q[t])) {
if (invalid_var(this->ekfConfiguration.Q[t])) {
return 2;
}
}
for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
if (invalid_var(ekfConfiguration.R[t])) {
if (invalid_var(this->ekfConfiguration.R[t])) {
return 2;
}
}
HomeLocationGet(&homeLocation);
HomeLocationGet(&this->homeLocation);
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) {
if ((this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2] < 1e-5f)) {
return 2;
}
@ -154,14 +179,9 @@ static int32_t maininit(void)
/**
* Collect all required state variables, then run complementary filter
*/
static int32_t filter(stateEstimation *state)
static int32_t filter(stateFilter *self, stateEstimation *state)
{
static int32_t init_stage;
static stateEstimation work;
static uint32_t ins_last_time = 0;
static bool inited;
struct data *this = (struct data *)self->localdata;
const float zeros[3] = { 0.0f, 0.0f, 0.0f };
@ -169,58 +189,52 @@ static int32_t filter(stateEstimation *state)
float dT;
uint16_t sensors = 0;
if (inited) {
work.updated = 0;
}
if (first_run) {
first_run = false;
inited = false;
init_stage = 0;
work.updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 1;
}
work.updated |= state->updated;
this->work.updated |= state->updated;
// Get most recent data
#define UPDATE(shortname, num) \
if (ISSET(state->updated, shortname##_UPDATED)) { \
if (ISSET(state->updated, SENSORUPDATES_##shortname)) { \
uint8_t t; \
for (t = 0; t < num; t++) { \
work.shortname[t] = state->shortname[t]; \
this->work.shortname[t] = state->shortname[t]; \
} \
}
UPDATE(gyr, 3);
UPDATE(acc, 3);
UPDATE(gyro, 3);
UPDATE(accel, 3);
UPDATE(mag, 3);
UPDATE(bar, 1);
UPDATE(baro, 1);
UPDATE(pos, 3);
UPDATE(vel, 3);
UPDATE(air, 2);
UPDATE(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
// ("gyros last" rule for multi object synchronization)
if (!(ISSET(this->work.updated, SENSORUPDATES_accel) && ISSET(state->updated, SENSORUPDATES_gyro))) {
UNSET(state->updated, SENSORUPDATES_pos);
UNSET(state->updated, SENSORUPDATES_vel);
UNSET(state->updated, SENSORUPDATES_attitude);
UNSET(state->updated, SENSORUPDATES_gyro);
return 0;
}
if (usePos) {
if (this->usePos) {
GPSPositionData gpsData;
GPSPositionGet(&gpsData);
// Have a minimum requirement for gps usage
if ((gpsData.Satellites < 7) ||
(gpsData.PDOP > 4.0f) ||
(gpsData.Latitude == 0 && gpsData.Longitude == 0) ||
(homeLocation.Set != HOMELOCATION_SET_TRUE)) {
UNSET(state->updated, pos_UPDATED);
UNSET(state->updated, vel_UPDATED);
UNSET(work.updated, pos_UPDATED);
UNSET(work.updated, vel_UPDATED);
(this->homeLocation.Set != HOMELOCATION_SET_TRUE)) {
UNSET(state->updated, SENSORUPDATES_pos);
UNSET(state->updated, SENSORUPDATES_vel);
UNSET(this->work.updated, SENSORUPDATES_pos);
UNSET(this->work.updated, SENSORUPDATES_vel);
}
}
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
dT = PIOS_DELAY_DiffuS(this->ins_last_time) / 1.0e6f;
this->ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if (dT > 0.01f) {
@ -229,28 +243,28 @@ static int32_t filter(stateEstimation *state)
dT = 0.001f;
}
if (!inited && ISSET(work.updated, mag_UPDATED) && ISSET(work.updated, bar_UPDATED) && ISSET(work.updated, pos_UPDATED)) {
if (!this->inited && ISSET(this->work.updated, SENSORUPDATES_mag) && ISSET(this->work.updated, SENSORUPDATES_baro) && ISSET(this->work.updated, SENSORUPDATES_pos)) {
// Don't initialize until all sensors are read
if (init_stage == 0) {
if (this->init_stage == 0) {
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] }
INSSetMagVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGX],
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGY],
this->ekfConfiguration.R[EKFCONFIGURATION_R_MAGZ] }
);
INSSetAccelVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELZ] }
);
INSSetGyroVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
INSSetGyroVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROZ] }
);
INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
INSSetGyroBiasVar((float[3]) { this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTY],
this->ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTZ] }
);
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
INSSetBaroVar(this->ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
// Initialize the gyro bias
float gyro_bias[3] = { 0.0f, 0.0f, 0.0f };
@ -261,15 +275,15 @@ static int32_t filter(stateEstimation *state)
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level
attitudeState.Roll = atan2f(-work.acc[1], -work.acc[2]);
float zn = cosf(attitudeState.Roll) * work.mag[2] + sinf(attitudeState.Roll) * work.mag[1];
float yn = cosf(attitudeState.Roll) * work.mag[1] - sinf(attitudeState.Roll) * work.mag[2];
attitudeState.Roll = atan2f(-this->work.accel[1], -this->work.accel[2]);
float zn = cosf(attitudeState.Roll) * this->work.mag[2] + sinf(attitudeState.Roll) * this->work.mag[1];
float yn = cosf(attitudeState.Roll) * this->work.mag[1] - sinf(attitudeState.Roll) * this->work.mag[2];
// rotate accels z vector according to roll
float azn = cosf(attitudeState.Roll) * work.acc[2] + sinf(attitudeState.Roll) * work.acc[1];
attitudeState.Pitch = atan2f(work.acc[0], -azn);
float azn = cosf(attitudeState.Roll) * this->work.accel[2] + sinf(attitudeState.Roll) * this->work.accel[1];
attitudeState.Pitch = atan2f(this->work.accel[0], -azn);
float xn = cosf(attitudeState.Pitch) * work.mag[0] + sinf(attitudeState.Pitch) * zn;
float xn = cosf(attitudeState.Pitch) * this->work.mag[0] + sinf(attitudeState.Pitch) * zn;
attitudeState.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack
@ -282,122 +296,125 @@ static int32_t filter(stateEstimation *state)
attitudeState.Pitch = RAD2DEG(attitudeState.Pitch);
attitudeState.Yaw = RAD2DEG(attitudeState.Yaw);
RPY2Quaternion(&attitudeState.Roll, work.att);
RPY2Quaternion(&attitudeState.Roll, this->work.attitude);
INSSetState(work.pos, (float *)zeros, work.att, (float *)zeros, (float *)zeros);
INSSetState(this->work.pos, (float *)zeros, this->work.attitude, (float *)zeros, (float *)zeros);
INSResetP(ekfConfiguration.P);
INSResetP(this->ekfConfiguration.P);
} else {
// Run prediction a bit before any corrections
float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) };
INSStatePrediction(gyros, work.acc, dT);
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
INSStatePrediction(gyros, this->work.accel, dT);
state->att[0] = Nav.q[0];
state->att[1] = Nav.q[1];
state->att[2] = Nav.q[2];
state->att[3] = Nav.q[3];
state->gyr[0] += Nav.gyro_bias[0];
state->gyr[1] += Nav.gyro_bias[1];
state->gyr[2] += Nav.gyro_bias[2];
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
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->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2];
state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED;
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
}
init_stage++;
if (init_stage > 10) {
inited = true;
this->init_stage++;
if (this->init_stage > 10) {
this->inited = true;
}
return 0;
}
if (!inited) {
if (!this->inited) {
return 1;
}
float gyros[3] = { DEG2RAD(work.gyr[0]), DEG2RAD(work.gyr[1]), DEG2RAD(work.gyr[2]) };
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) };
// Advance the state estimate
INSStatePrediction(gyros, work.acc, dT);
INSStatePrediction(gyros, this->work.accel, dT);
// Copy the attitude into the UAVO
state->att[0] = Nav.q[0];
state->att[1] = Nav.q[1];
state->att[2] = Nav.q[2];
state->att[3] = Nav.q[3];
state->gyr[0] += Nav.gyro_bias[0];
state->gyr[1] += Nav.gyro_bias[1];
state->gyr[2] += Nav.gyro_bias[2];
// Copy the attitude into the state
// NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true
state->attitude[0] = Nav.q[0];
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->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
state->vel[0] = Nav.Vel[0];
state->vel[1] = Nav.Vel[1];
state->vel[2] = Nav.Vel[2];
state->updated |= att_UPDATED | pos_UPDATED | vel_UPDATED | gyr_UPDATED;
state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel;
// Advance the covariance estimate
INSCovariancePrediction(dT);
if (ISSET(work.updated, mag_UPDATED)) {
if (ISSET(this->work.updated, SENSORUPDATES_mag)) {
sensors |= MAG_SENSORS;
}
if (ISSET(work.updated, bar_UPDATED)) {
if (ISSET(this->work.updated, SENSORUPDATES_baro)) {
sensors |= BARO_SENSOR;
}
INSSetMagNorth(homeLocation.Be);
INSSetMagNorth(this->homeLocation.Be);
if (!usePos) {
if (!this->usePos) {
// position and velocity variance used in indoor mode
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELINDOOR] }
);
} else {
// position and velocity variance used in outdoor mode
INSSetPosVelVar((float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN] },
(float[3]) { this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST],
this->ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN] }
);
}
if (ISSET(work.updated, pos_UPDATED)) {
if (ISSET(this->work.updated, SENSORUPDATES_pos)) {
sensors |= POS_SENSORS;
}
if (ISSET(work.updated, vel_UPDATED)) {
if (ISSET(this->work.updated, SENSORUPDATES_vel)) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
}
if (ISSET(work.updated, air_UPDATED) && ((!ISSET(work.updated, vel_UPDATED) && !ISSET(work.updated, pos_UPDATED)) | !usePos)) {
if (ISSET(this->work.updated, SENSORUPDATES_airspeed) && ((!ISSET(this->work.updated, SENSORUPDATES_vel) && !ISSET(this->work.updated, SENSORUPDATES_pos)) | !this->usePos)) {
// HACK: feed airspeed into EKF as velocity, treat wind as 1e2 variance
sensors |= HORIZ_SENSORS | VERT_SENSORS;
INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
INSSetPosVelVar((float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSPOSINDOOR] },
(float[3]) { this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED],
this->ekfConfiguration.FakeR[EKFCONFIGURATION_FAKER_FAKEGPSVELAIRSPEED] }
);
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
float R[3][3];
Quaternion2R(Nav.q, R);
float vtas[3] = { work.air[1], 0.0f, 0.0f };
rot_mult(R, vtas, work.vel);
float vtas[3] = { this->work.airspeed[1], 0.0f, 0.0f };
rot_mult(R, vtas, this->work.vel);
}
/*
@ -405,7 +422,7 @@ static int32_t filter(stateEstimation *state)
* although probably should occur within INS itself
*/
if (sensors) {
INSCorrection(work.mag, work.pos, work.vel, work.bar[0], sensors);
INSCorrection(this->work.mag, this->work.pos, this->work.vel, this->work.baro[0], sensors);
}
EKFStateVarianceData vardata;
@ -413,22 +430,16 @@ static int32_t filter(stateEstimation *state)
INSGetP(vardata.P);
EKFStateVarianceSet(&vardata);
return 0;
}
// all sensor data has been used, reset!
this->work.updated = 0;
// check for invalid values
static inline bool invalid(float data)
{
if (isnan(data) || isinf(data)) {
return true;
}
return false;
return 0;
}
// check for invalid variance values
static inline bool invalid_var(float data)
{
if (invalid(data)) {
if (isnan(data) || isinf(data)) {
return true;
}
if (data < 1e-15f) { // var should not be close to zero. And not negative either.

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

@ -38,41 +38,50 @@
#include <CoordinateConversions.h>
// Private constants
//
#define STACK_REQUIRED 256
// Private types
struct data {
HomeLocationData homeLocation;
RevoCalibrationData revoCalibration;
float magBias[3];
};
// Private variables
static HomeLocationData homeLocation;
static RevoCalibrationData revoCalibration;
static float magBias[3] = { 0 };
// Private functions
static int32_t init(void);
static int32_t filter(stateEstimation *state);
static void magOffsetEstimation(float mag[3]);
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
static void magOffsetEstimation(struct data *this, float mag[3]);
void filterMagInitialize(stateFilter *handle)
int32_t filterMagInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
return STACK_REQUIRED;
}
static int32_t init(void)
static int32_t init(stateFilter *self)
{
magBias[0] = magBias[1] = magBias[2] = 0.0f;
HomeLocationGet(&homeLocation);
RevoCalibrationGet(&revoCalibration);
struct data *this = (struct data *)self->localdata;
this->magBias[0] = this->magBias[1] = this->magBias[2] = 0.0f;
HomeLocationGet(&this->homeLocation);
RevoCalibrationGet(&this->revoCalibration);
return 0;
}
static int32_t filter(stateEstimation *state)
static int32_t filter(stateFilter *self, stateEstimation *state)
{
if (ISSET(state->updated, mag_UPDATED)) {
if (revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(state->mag);
struct data *this = (struct data *)self->localdata;
if (ISSET(state->updated, SENSORUPDATES_mag)) {
if (this->revoCalibration.MagBiasNullingRate > 0) {
magOffsetEstimation(this, state->mag);
}
}
@ -84,7 +93,7 @@ static int32_t filter(stateEstimation *state)
* Magmeter Offset Cancellation: Theory and Implementation,
* revisited William Premerlani, October 14, 2011
*/
static void magOffsetEstimation(float mag[3])
static void magOffsetEstimation(struct data *this, float mag[3])
{
#if 0
// Constants, to possibly go into a UAVO
@ -127,10 +136,10 @@ static void magOffsetEstimation(float mag[3])
}
#else // if 0
const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]);
const float Rz = homeLocation.Be[2];
const float Rxy = sqrtf(this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1]);
const float Rz = this->homeLocation.Be[2];
const float rate = revoCalibration.MagBiasNullingRate;
const float rate = this->revoCalibration.MagBiasNullingRate;
float Rot[3][3];
float B_e[3];
float xy[2];
@ -162,15 +171,15 @@ static void magOffsetEstimation(float mag[3])
if (!isnan(delta[0]) && !isinf(delta[0]) &&
!isnan(delta[1]) && !isinf(delta[1]) &&
!isnan(delta[2]) && !isinf(delta[2])) {
magBias[0] += delta[0];
magBias[1] += delta[1];
magBias[2] += delta[2];
this->magBias[0] += delta[0];
this->magBias[1] += delta[1];
this->magBias[2] += delta[2];
}
// Add bias to state estimation
mag[0] += magBias[0];
mag[1] += magBias[1];
mag[2] += magBias[2];
mag[0] += this->magBias[0];
mag[1] += this->magBias[1];
mag[2] += this->magBias[2];
#endif // if 0
}

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

@ -33,38 +33,42 @@
// Private constants
#define STACK_REQUIRED 64
// Private types
// Private variables
// Private functions
static int32_t init(void);
static int32_t filter(stateEstimation *state);
static int32_t init(stateFilter *self);
static int32_t filter(stateFilter *self, stateEstimation *state);
void filterStationaryInitialize(stateFilter *handle)
int32_t filterStationaryInitialize(stateFilter *handle)
{
handle->init = &init;
handle->filter = &filter;
handle->localdata = NULL;
return STACK_REQUIRED;
}
static int32_t init(void)
static int32_t init(__attribute__((unused)) stateFilter *self)
{
return 0;
}
static int32_t filter(stateEstimation *state)
static int32_t filter(__attribute__((unused)) stateFilter *self, stateEstimation *state)
{
state->pos[0] = 0.0f;
state->pos[1] = 0.0f;
state->pos[2] = 0.0f;
state->updated |= pos_UPDATED;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = 0.0f;
state->vel[1] = 0.0f;
state->vel[2] = 0.0f;
state->updated |= vel_UPDATED;
state->updated |= SENSORUPDATES_vel;
return 0;
}

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

@ -42,7 +42,6 @@
#include <gyrostate.h>
#include <accelstate.h>
#include <magstate.h>
#include <barostate.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <positionstate.h>
@ -55,7 +54,7 @@
#include "CoordinateConversions.h"
// Private constants
#define STACK_SIZE_BYTES 2048
#define STACK_SIZE_BYTES 256
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define TASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define TIMEOUT_MS 100
@ -185,7 +184,7 @@ 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)
static inline int32_t maxint32_t(int32_t a, int32_t b)
{
if (a > b) {
return a;
@ -212,7 +211,6 @@ int32_t StateEstimationInitialize(void)
GyroStateInitialize();
AccelStateInitialize();
MagStateInitialize();
BaroStateInitialize();
AirspeedStateInitialize();
PositionStateInitialize();
VelocityStateInitialize();
@ -316,7 +314,7 @@ static void StateEstimationCb(void)
current = (filterPipeline *)newFilterChain;
bool error = 0;
while (current != NULL) {
int32_t result = current->filter->init();
int32_t result = current->filter->init((stateFilter *)current->filter);
if (result != 0) {
error = 1;
break;
@ -340,7 +338,7 @@ static void StateEstimationCb(void)
// most sensors get only rudimentary sanity checks
#define SANITYCHECK3(sensorname, shortname, a1, a2, a3) \
if (ISSET(states.updated, shortname##_UPDATED)) { \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (sane(s.a1) && sane(s.a2) && sane(s.a3)) { \
@ -349,36 +347,36 @@ static void StateEstimationCb(void)
states.shortname[2] = s.a3; \
} \
else { \
UNSET(states.updated, shortname##_UPDATED); \
UNSET(states.updated, SENSORUPDATES_##shortname); \
} \
}
SANITYCHECK3(GyroSensor, gyr, x, y, z);
SANITYCHECK3(AccelSensor, acc, x, y, z);
SANITYCHECK3(GyroSensor, gyro, x, y, z);
SANITYCHECK3(AccelSensor, accel, 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)) { \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
sensorname##Data s; \
sensorname##Get(&s); \
if (sane(s.a1) && EXTRACHECK) { \
states.shortname[0] = s.a1; \
} \
else { \
UNSET(states.updated, shortname##_UPDATED); \
UNSET(states.updated, SENSORUPDATES_##shortname); \
} \
}
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, baro, Altitude, 1);
SANITYCHECK1(AirspeedSensor, airspeed, CalibratedAirspeed, s.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE);
states.airspeed[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)) {
if (ISSET(states.updated, SENSORUPDATES_pos)) {
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);
UNSET(states.updated, SENSORUPDATES_pos);
}
}
@ -395,7 +393,7 @@ static void StateEstimationCb(void)
case RUNSTATE_FILTER:
if (current != NULL) {
int32_t result = current->filter->filter(&states);
int32_t result = current->filter->filter((stateFilter *)current->filter, &states);
if (result > alarm) {
alarm = result;
}
@ -413,7 +411,7 @@ static void StateEstimationCb(void)
// the final output of filters is saved in state variables
#define STORE3(statename, shortname, a1, a2, a3) \
if (ISSET(states.updated, shortname##_UPDATED)) { \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
@ -421,36 +419,28 @@ 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(GyroState, gyro, x, y, z);
STORE3(AccelState, accel, 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)) { \
if (ISSET(states.updated, SENSORUPDATES_##shortname)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
s.a2 = states.shortname[1]; \
statename##Set(&s); \
}
STORE2(AirspeedState, air, CalibratedAirspeed, TrueAirspeed);
#define STORE1(statename, shortname, a1) \
if (ISSET(states.updated, shortname##_UPDATED)) { \
statename##Data s; \
statename##Get(&s); \
s.a1 = states.shortname[0]; \
statename##Set(&s); \
}
STORE1(BaroState, bar, Altitude);
STORE2(AirspeedState, airspeed, CalibratedAirspeed, TrueAirspeed);
// attitude nees manual conversion from quaternion to euler
if (ISSET(states.updated, att_UPDATED)) { \
if (ISSET(states.updated, SENSORUPDATES_attitude)) { \
AttitudeStateData s;
AttitudeStateGet(&s);
s.q1 = states.att[0];
s.q2 = states.att[1];
s.q3 = states.att[2];
s.q4 = states.att[3];
s.q1 = states.attitude[0];
s.q2 = states.attitude[1];
s.q3 = states.attitude[2];
s.q4 = states.attitude[3];
Quaternion2RPY(&s.q1, &s.Roll);
AttitudeStateSet(&s);
}
@ -528,31 +518,31 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
}
if (ev->obj == GyroSensorHandle()) {
updatedSensors |= gyr_UPDATED;
updatedSensors |= SENSORUPDATES_gyro;
}
if (ev->obj == AccelSensorHandle()) {
updatedSensors |= acc_UPDATED;
updatedSensors |= SENSORUPDATES_accel;
}
if (ev->obj == MagSensorHandle()) {
updatedSensors |= mag_UPDATED;
updatedSensors |= SENSORUPDATES_mag;
}
if (ev->obj == GPSPositionHandle()) {
updatedSensors |= pos_UPDATED;
updatedSensors |= SENSORUPDATES_pos;
}
if (ev->obj == GPSVelocityHandle()) {
updatedSensors |= vel_UPDATED;
updatedSensors |= SENSORUPDATES_vel;
}
if (ev->obj == BaroSensorHandle()) {
updatedSensors |= bar_UPDATED;
updatedSensors |= SENSORUPDATES_baro;
}
if (ev->obj == AirspeedSensorHandle()) {
updatedSensors |= air_UPDATED;
updatedSensors |= SENSORUPDATES_airspeed;
}
DelayedCallbackDispatch(stateEstimationCallback);

1
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -33,7 +33,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate

1
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate

1
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor
UAVOBJSRCFILENAMES += magsensor
UAVOBJSRCFILENAMES += magstate
UAVOBJSRCFILENAMES += barosensor
UAVOBJSRCFILENAMES += barostate
UAVOBJSRCFILENAMES += airspeedsensor
UAVOBJSRCFILENAMES += airspeedsettings
UAVOBJSRCFILENAMES += airspeedstate

2
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -25,7 +25,6 @@ OTHER_FILES += UAVObjects.pluginspec
# Add in all of the synthetic/generated uavobject files
HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/barosensor.h \
$$UAVOBJECT_SYNTHETICS/barostate.h \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.h \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.h \
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
@ -110,7 +109,6 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \
$$UAVOBJECT_SYNTHETICS/barostate.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsensor.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedsettings.cpp \
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \

10
shared/uavobjectdefinition/barostate.xml
View File

@ -1,10 +0,0 @@
<xml>
<object name="BaroState" singleinstance="true" settings="false">
<description>The filtered altitude estimate.</description>
<field name="Altitude" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>