From b56de3b66b76f0d0817e9b3173669ce18bf1cdf3 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 May 2013 21:11:48 +0200 Subject: [PATCH] filterchain rework --- flight/modules/StateEstimation/filterair.c | 29 +- flight/modules/StateEstimation/filterbaro.c | 52 +-- flight/modules/StateEstimation/filtercf.c | 284 ++++++++------- flight/modules/StateEstimation/filterekf.c | 333 +++++++++--------- flight/modules/StateEstimation/filtermag.c | 65 ++-- .../StateEstimation/filterstationary.c | 22 +- .../modules/StateEstimation/stateestimation.c | 74 ++-- .../boards/revolution/firmware/UAVObjects.inc | 1 - .../boards/revoproto/firmware/UAVObjects.inc | 1 - .../boards/simposix/firmware/UAVObjects.inc | 1 - .../src/plugins/uavobjects/uavobjects.pro | 2 - shared/uavobjectdefinition/barostate.xml | 10 - 12 files changed, 450 insertions(+), 424 deletions(-) delete mode 100644 shared/uavobjectdefinition/barostate.xml diff --git a/flight/modules/StateEstimation/filterair.c b/flight/modules/StateEstimation/filterair.c index 1cf875493..f5a4ee60d 100644 --- a/flight/modules/StateEstimation/filterair.c +++ b/flight/modules/StateEstimation/filterair.c @@ -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->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; diff --git a/flight/modules/StateEstimation/filterbaro.c b/flight/modules/StateEstimation/filterbaro.c index 6cda6b175..b8b8f14be 100644 --- a/flight/modules/StateEstimation/filterbaro.c +++ b/flight/modules/StateEstimation/filterbaro.c @@ -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->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 + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-state->pos[2] - state->bar[0]); + if (ISSET(state->updated, SENSORUPDATES_pos)) { + this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + + (1.0f - BARO_OFFSET_LOWPASS_ALPHA) + * (-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; } } diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 0a3c599cd..27ebbcce9 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -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->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->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) { - int32_t result = 0; + struct data *this = (struct data *)self->localdata; - if (ISSET(state->updated, mag_UPDATED)) { - magUpdated = 1; - currentMag[0] = state->mag[0]; - currentMag[1] = state->mag[1]; - currentMag[2] = state->mag[2]; + int32_t result = 0; + + 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; } diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index 3d6271fda..e6e20e648 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -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->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->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->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->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. diff --git a/flight/modules/StateEstimation/filtermag.c b/flight/modules/StateEstimation/filtermag.c index a54e99257..ceec76747 100644 --- a/flight/modules/StateEstimation/filtermag.c +++ b/flight/modules/StateEstimation/filtermag.c @@ -38,41 +38,50 @@ #include // 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->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 } diff --git a/flight/modules/StateEstimation/filterstationary.c b/flight/modules/StateEstimation/filterstationary.c index b33e3b9f7..14075fd1c 100644 --- a/flight/modules/StateEstimation/filterstationary.c +++ b/flight/modules/StateEstimation/filterstationary.c @@ -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->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; } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 2c039dd8d..dd8247d71 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -42,7 +42,6 @@ #include #include #include -#include #include #include #include @@ -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); diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 820187757..1ee34b95d 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -33,7 +33,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index d516cada5..5742838ba 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index dbaf20e87..5134a536d 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -38,7 +38,6 @@ UAVOBJSRCFILENAMES += accelsensor UAVOBJSRCFILENAMES += magsensor UAVOBJSRCFILENAMES += magstate UAVOBJSRCFILENAMES += barosensor -UAVOBJSRCFILENAMES += barostate UAVOBJSRCFILENAMES += airspeedsensor UAVOBJSRCFILENAMES += airspeedsettings UAVOBJSRCFILENAMES += airspeedstate diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d588782cf..4ff9d4050 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -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 \ diff --git a/shared/uavobjectdefinition/barostate.xml b/shared/uavobjectdefinition/barostate.xml deleted file mode 100644 index 56a331776..000000000 --- a/shared/uavobjectdefinition/barostate.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - The filtered altitude estimate. - - - - - - -