From bc3636bbd673f81b44e030cf74858909de1a6d23 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 23 Feb 2014 13:04:39 +0100 Subject: [PATCH 1/5] OP-1235 gives better names to state variables Conflicts: flight/modules/StateEstimation/filteraltitude.c --- .../modules/StateEstimation/filteraltitude.c | 41 ++++++++++--------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 2b46b35af..aba721ec6 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -47,7 +47,10 @@ // Private types struct data { - float state[4]; // state = altitude,velocity,accel_offset,accel + float altitudeState; // state = altitude,velocity,accel_offset,accel + float velocityState; + float accelBiasState; + float accelState; float pos[3]; // position updates from other filters float vel[3]; // position updates from other filters @@ -81,10 +84,10 @@ static int32_t init(stateFilter *self) { struct data *this = (struct data *)self->localdata; - this->state[0] = 0.0f; - this->state[1] = 0.0f; - this->state[2] = 0.0f; - this->state[3] = 0.0f; + this->altitudeState = 0.0f; + this->velocityState = 0.0f; + this->accelBiasState = 0.0f; + this->accelState = 0.0f; this->pos[0] = 0.0f; this->pos[1] = 0.0f; this->pos[2] = 0.0f; @@ -115,13 +118,13 @@ static int32_t filter(stateFilter *self, stateEstimation *state) this->pos[0] = state->pos[0]; this->pos[1] = state->pos[1]; this->pos[2] = state->pos[2]; - state->pos[2] = -this->state[0]; + state->pos[2] = -this->altitudeState; } if (IS_SET(state->updated, SENSORUPDATES_vel)) { this->vel[0] = state->vel[0]; this->vel[1] = state->vel[1]; this->vel[2] = state->vel[2]; - state->vel[2] = -this->state[1]; + state->vel[2] = -this->velocityState; } if (IS_SET(state->updated, SENSORUPDATES_accel)) { // rotate accels into global coordinate frame @@ -132,51 +135,51 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); // low pass filter accelerometers - this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + this->settings.AccelLowPassKp * current; + this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; // correct accel offset (low pass zeroing) - this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3]; + this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler // float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config); - float speedLast = this->state[1]; + float speedLast = this->velocityState; - this->state[1] += 0.5f * (this->accelLast + (this->state[3] - this->state[2])) * dT; - this->accelLast = this->state[3] - this->state[2]; + this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT; + this->accelLast = this->accelState - this->accelBiasState; - this->state[0] += 0.5f * (speedLast + this->state[1]) * dT; + this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT; state->pos[0] = this->pos[0]; state->pos[1] = this->pos[1]; - state->pos[2] = -this->state[0]; + state->pos[2] = -this->altitudeState; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; state->vel[1] = this->vel[1]; - state->vel[2] = -this->state[1]; + state->vel[2] = -this->velocityState; state->updated |= SENSORUPDATES_vel; } if (IS_SET(state->updated, SENSORUPDATES_baro)) { // correct the altitude state (simple low pass) - this->state[0] = (1.0f - this->settings.BaroKp) * this->state[0] + this->settings.BaroKp * state->baro[0]; + this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0]; // correct the velocity state (low pass differentiation) // low pass for average dT, compensate timing jitter from scheduler 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->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT; this->baroLast = state->baro[0]; state->pos[0] = this->pos[0]; state->pos[1] = this->pos[1]; - state->pos[2] = -this->state[0]; + state->pos[2] = -this->altitudeState; state->updated |= SENSORUPDATES_pos; state->vel[0] = this->vel[0]; state->vel[1] = this->vel[1]; - state->vel[2] = -this->state[1]; + state->vel[2] = -this->velocityState; state->updated |= SENSORUPDATES_vel; } } From a2386f548b2aed950f0b71881798d5d670041039 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 22 Feb 2014 21:24:27 +0100 Subject: [PATCH 2/5] OP-1235 allow for live update of altitude estimation settings --- flight/modules/StateEstimation/filteraltitude.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index aba721ec6..d05caaff6 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -44,6 +44,7 @@ #define DT_MIN 1e-6f #define DT_MAX 1.0f #define DT_AVERAGE 1e-3f +static volatile bool reloadSettings; // Private types struct data { @@ -68,6 +69,7 @@ struct data { static int32_t init(stateFilter *self); static int32_t filter(stateFilter *self, stateEstimation *state); +static void settingsUpdatedCb(UAVObjEvent *ev); int32_t filterAltitudeInitialize(stateFilter *handle) @@ -77,6 +79,8 @@ int32_t filterAltitudeInitialize(stateFilter *handle) handle->localdata = pvPortMalloc(sizeof(struct data)); AttitudeStateInitialize(); AltitudeFilterSettingsInitialize(); + AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb); + reloadSettings = true; return STACK_REQUIRED; } @@ -99,7 +103,6 @@ static int32_t init(stateFilter *self) this->baroLast = 0.0f; this->accelLast = 0.0f; this->first_run = 1; - AltitudeFilterSettingsGet(&this->settings); return 0; } @@ -107,6 +110,11 @@ static int32_t filter(stateFilter *self, stateEstimation *state) { struct data *this = (struct data *)self->localdata; + if (reloadSettings) { + reloadSettings = false; + AltitudeFilterSettingsGet(&this->settings); + } + if (this->first_run) { // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { @@ -187,6 +195,12 @@ static int32_t filter(stateFilter *self, stateEstimation *state) return 0; } +void settingsUpdatedCb(UAVObjEvent *ev) +{ + if (ev->obj == AltitudeFilterSettingsHandle()) { + reloadSettings = true; + } +} /** * @} From 7f6175d81773e93180419a38b663e2c62b98d0b2 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 23 Feb 2014 12:57:59 +0100 Subject: [PATCH 3/5] OP-1235 allows AccelBiasValue to quickly reach the target value at initialization --- flight/modules/StateEstimation/filteraltitude.c | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index d05caaff6..0bea2411f 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -60,6 +60,7 @@ struct data { float accelLast; float baroLast; bool first_run; + portTickType initTimer; AltitudeFilterSettingsData settings; }; @@ -119,6 +120,7 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // Initialize to current altitude reading at initial location if (IS_SET(state->updated, SENSORUPDATES_baro)) { this->first_run = 0; + this->initTimer = xTaskGetTickCount(); } } else { // save existing position and velocity updates so GPS will still work @@ -143,11 +145,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state) float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); // low pass filter accelerometers - this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; - - // correct accel offset (low pass zeroing) - this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; - + this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; + if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < 5000) { + // allow the offset to reach quickly the target value in case of small AccelDriftKi + this->accelBiasState = (1.0f - 0.2f) * this->accelBiasState + 0.2f * this->accelState; + } else { + // correct accel offset (low pass zeroing) + this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; + } // correct velocity and position state (integration) // low pass for average dT, compensate timing jitter from scheduler // From 4764186a5ff353623cd2f0ef8e89c7545d70755d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Apr 2014 20:51:16 +0200 Subject: [PATCH 4/5] OP-1235 cleanup "magic numbers" --- flight/modules/StateEstimation/filteraltitude.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 0bea2411f..3b4a047f8 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -38,6 +38,12 @@ // Private constants +// Acceldrift ki value used at startup to let it quickly converge +// to its target value +#define INITIALIZATION_ACCELDRIFT_KI 0.2f +// lenght of accel bias initialization phase +#define INITIALIZATION_DURATION 5000 + #define STACK_REQUIRED 128 #define DT_ALPHA 1e-2f @@ -146,9 +152,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // low pass filter accelerometers this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; - if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < 5000) { + if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION) { // allow the offset to reach quickly the target value in case of small AccelDriftKi - this->accelBiasState = (1.0f - 0.2f) * this->accelBiasState + 0.2f * this->accelState; + this->accelBiasState = (1.0f - INITIALIZATION_ACCELDRIFT_KI) * this->accelBiasState + INITIALIZATION_ACCELDRIFT_KI * this->accelState; } else { // correct accel offset (low pass zeroing) this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; From 5763ed3d8d1914db7c012875cb5526b26e535581 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Mon, 14 Apr 2014 10:58:46 +0200 Subject: [PATCH 5/5] OP-1235 make Initialization Acceldrift KI configurable --- flight/modules/StateEstimation/filteraltitude.c | 11 ++++------- shared/uavobjectdefinition/altitudefiltersettings.xml | 1 + 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/flight/modules/StateEstimation/filteraltitude.c b/flight/modules/StateEstimation/filteraltitude.c index 3b4a047f8..26f9598b4 100644 --- a/flight/modules/StateEstimation/filteraltitude.c +++ b/flight/modules/StateEstimation/filteraltitude.c @@ -38,11 +38,8 @@ // Private constants -// Acceldrift ki value used at startup to let it quickly converge -// to its target value -#define INITIALIZATION_ACCELDRIFT_KI 0.2f -// lenght of accel bias initialization phase -#define INITIALIZATION_DURATION 5000 +// duration of accel bias initialization phase +#define INITIALIZATION_DURATION_MS 5000 #define STACK_REQUIRED 128 @@ -152,9 +149,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state) // low pass filter accelerometers this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current; - if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION) { + if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) { // allow the offset to reach quickly the target value in case of small AccelDriftKi - this->accelBiasState = (1.0f - INITIALIZATION_ACCELDRIFT_KI) * this->accelBiasState + INITIALIZATION_ACCELDRIFT_KI * this->accelState; + this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState; } else { // correct accel offset (low pass zeroing) this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState; diff --git a/shared/uavobjectdefinition/altitudefiltersettings.xml b/shared/uavobjectdefinition/altitudefiltersettings.xml index 9b3eb0838..4bbfa33e5 100644 --- a/shared/uavobjectdefinition/altitudefiltersettings.xml +++ b/shared/uavobjectdefinition/altitudefiltersettings.xml @@ -3,6 +3,7 @@ Settings for the @ref State Estimator module plugin altitudeFilter +