From 28be9cc8ce177572889b2bae00a5868b181a1abb Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 19:48:33 +0100 Subject: [PATCH] OP-1211 adapted existing code to use new pios functionality instead of separate error prone implementations --- flight/modules/Attitude/attitude.c | 15 +++-- flight/modules/Stabilization/stabilization.c | 1 - .../modules/StateEstimation/filteraltitude.c | 63 +++++++------------ flight/modules/StateEstimation/filterekf.c | 34 ++++------ 4 files changed, 43 insertions(+), 70 deletions(-) diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index 84ee51bb3..6a2bb4ce7 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -72,10 +72,16 @@ #define UPDATE_RATE 25.0f #define GYRO_NEUTRAL 1665 +#define UPDATE_EXPECTED (1.0f / 666.0f) +#define UPDATE_MIN 1.0e-6f +#define UPDATE_MAX 1.0f +#define UPDATE_ALPHA 1.0e-2f + // Private types // Private variables static xTaskHandle taskHandle; +static PiOSDeltatimeConfig dtconfig; // Private functions static void AttitudeTask(void *parameters); @@ -214,6 +220,8 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); + PIOS_DELTATIME_Init(&dtconfig, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); + // Main task loop while (1) { FlightStatusData flightStatus; @@ -473,12 +481,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) { - float dT; - portTickType thisSysTime = xTaskGetTickCount(); - static portTickType lastSysTime = 0; - - dT = (thisSysTime == lastSysTime) ? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f; - lastSysTime = thisSysTime; + float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); // Bad practice to assume structure order, but saves memory float *gyros = &gyrosData->x; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index c8da3c487..ab5c7ab61 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -234,7 +234,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - fprintf(stderr, "dt is %f\n", dT); FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeStateGet(&attitudeState); diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 01d38a1b8..2b46b35af 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -40,20 +40,22 @@ #define STACK_REQUIRED 128 -#define DT_ALPHA 1e-3f +#define DT_ALPHA 1e-2f +#define DT_MIN 1e-6f +#define DT_MAX 1.0f +#define DT_AVERAGE 1e-3f // Private types struct data { - float state[4]; // state = altitude,velocity,accel_offset,accel - float pos[3]; // position updates from other filters - float vel[3]; // position updates from other filters - float dTA; - float dTA2; - int32_t lastTime; - float accelLast; - float baroLast; - int32_t baroLastTime; - bool first_run; + float state[4]; // state = altitude,velocity,accel_offset,accel + float pos[3]; // position updates from other filters + float vel[3]; // position updates from other filters + + PiOSDeltatimeConfig dt1config; + PiOSDeltatimeConfig dt2config; + float accelLast; + float baroLast; + bool first_run; AltitudeFilterSettingsData settings; }; @@ -89,8 +91,8 @@ static int32_t init(stateFilter *self) this->vel[0] = 0.0f; this->vel[1] = 0.0f; this->vel[2] = 0.0f; - this->dTA = -1.0f; - this->dTA2 = -1.0f; + PIOS_DELTATIME_Init(&this->dt1config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA); + PIOS_DELTATIME_Init(&this->dt2config, DT_AVERAGE, DT_MIN, DT_MAX, DT_ALPHA); this->baroLast = 0.0f; this->accelLast = 0.0f; this->first_run = 1; @@ -104,12 +106,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) if (this->first_run) { // Initialize to current altitude reading at initial location - if (IS_SET(state->updated, SENSORUPDATES_accel)) { - this->lastTime = PIOS_DELAY_GetRaw(); - } if (IS_SET(state->updated, SENSORUPDATES_baro)) { - this->first_run = 0; - this->baroLastTime = PIOS_DELAY_GetRaw(); + this->first_run = 0; } } else { // save existing position and velocity updates so GPS will still work @@ -141,22 +139,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler - float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f; - this->lastTime = PIOS_DELAY_GetRaw(); - if (dT < 0.001f) { - dT = 0.001f; - } - if (this->dTA < 0) { - this->dTA = dT; - } else { - this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA; - } + // + float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config); float speedLast = this->state[1]; - this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * this->dTA; + this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT; this->accelLast = this->state[3] - this->state[2]; - this->state[0] += 0.5f * (speedLast + this->state[1]) * this->dTA; + this->state[0] += 0.5f * (speedLast + this->state[1]) * dT; state->pos[0] = this->pos[0]; @@ -175,17 +165,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // correct the velocity state (low pass differentiation) // low pass for average dT, compensate timing jitter from scheduler - float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f; - this->baroLastTime = PIOS_DELAY_GetRaw(); - if (dT < 0.001f) { - dT = 0.001f; - } - if (this->dTA2 < 0) { - this->dTA2 = dT; - } else { - this->dTA2 = this->dTA2 * (1.0f - DT_ALPHA) + dT * DT_ALPHA; - } - this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / this->dTA2; + float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config); + this->state[1] = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->state[1] + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT; this->baroLast = state->baro[0]; state->pos[0] = this->pos[0]; diff --git a/flight/modules/StateEstimation/filterekf.c b/flight/modules/StateEstimation/filterekf.c index a141e4d8f..0aa906dda 100644 --- a/flight/modules/StateEstimation/filterekf.c +++ b/flight/modules/StateEstimation/filterekf.c @@ -45,6 +45,8 @@ #define STACK_REQUIRED 2048 #define DT_ALPHA 1e-3f +#define DT_MIN 1e-6f +#define DT_MAX 1.0f #define DT_INIT (1.0f / 666.0f) // initialize with 666 Hz (default sensor update rate on revo) #define IMPORT_SENSOR_IF_UPDATED(shortname, num) \ @@ -66,10 +68,9 @@ struct data { stateEstimation work; - uint32_t ins_last_time; - bool inited; + bool inited; - float dTa; + PiOSDeltatimeConfig dtconfig; }; // Private variables @@ -154,11 +155,10 @@ static int32_t maininit(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->inited = false; - this->init_stage = 0; - this->work.updated = 0; - this->ins_last_time = PIOS_DELAY_GetRaw(); - this->dTa = DT_INIT; + this->inited = false; + this->init_stage = 0; + this->work.updated = 0; + PIOS_DELTATIME_Init(&this->dtconfig, DT_INIT, DT_MIN, DT_MAX, DT_ALPHA); EKFConfigurationGet(&this->ekfConfiguration); int t; @@ -224,17 +224,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } - 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) { - dT = 0.01f; - } else if (dT <= 0.001f) { - dT = 0.001f; - } - - this->dTa = this->dTa * (1.0f - DT_ALPHA) + dT * DT_ALPHA; // low pass for average dT, compensate timing jitter from scheduler + dT = PIOS_DELTATIME_GetAverageSeconds(&this->dtconfig); if (!this->inited && IS_SET(this->work.updated, SENSORUPDATES_mag) && IS_SET(this->work.updated, SENSORUPDATES_baro) && IS_SET(this->work.updated, SENSORUPDATES_pos)) { // Don't initialize until all sensors are read @@ -300,7 +290,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Run prediction a bit before any corrections float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; - INSStatePrediction(gyros, this->work.accel, this->dTa); + INSStatePrediction(gyros, this->work.accel, dT); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -335,7 +325,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; // Advance the state estimate - INSStatePrediction(gyros, this->work.accel, this->dTa); + INSStatePrediction(gyros, this->work.accel, dT); // Copy the attitude into the state // NOTE: updating gyr correctly is valid, because this code is reached only when SENSORUPDATES_gyro is already true @@ -355,7 +345,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) state->updated |= SENSORUPDATES_attitude | SENSORUPDATES_pos | SENSORUPDATES_vel; // Advance the covariance estimate - INSCovariancePrediction(this->dTa); + INSCovariancePrediction(dT); if (IS_SET(this->work.updated, SENSORUPDATES_mag)) { sensors |= MAG_SENSORS;