rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1308 merging filterlaltitude with OP-1235

Browse Source
This commit is contained in:
Bertrand Oresve 2014-05-05 21:33:57 +02:00
commit b2ebb1faf8
1 changed files with 58 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
flight/modules/StateEstimation/filteraltitude.c
View File

@ -33,21 +33,29 @@
#include "inc/stateestimation.h" #include "inc/stateestimation.h"
#include <attitudestate.h> #include <attitudestate.h>
#include <altitudefiltersettings.h> #include <altitudefiltersettings.h>
#include <homelocation.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
// Private constants // Private constants
#define STACK_REQUIRED 128 // duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define DT_ALPHA 1e-2f #define STACK_REQUIRED 128
#define DT_MIN 1e-6f
#define DT_MAX 1.0f #define DT_ALPHA 1e-2f
#define DT_AVERAGE 1e-3f #define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
static volatile bool reloadSettings;
// Private types // Private types
struct data { 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 pos[3]; // position updates from other filters
float vel[3]; // position updates from other filters float vel[3]; // position updates from other filters
@ -56,7 +64,9 @@ struct data {
float accelLast; float accelLast;
float baroLast; float baroLast;
bool first_run; bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings; AltitudeFilterSettingsData settings;
float gravity;
}; };
// Private variables // Private variables
@ -65,6 +75,7 @@ struct data {
static int32_t init(stateFilter *self); static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state); static filterResult filter(stateFilter *self, stateEstimation *state);
static void settingsUpdatedCb(UAVObjEvent *ev);
int32_t filterAltitudeInitialize(stateFilter *handle) int32_t filterAltitudeInitialize(stateFilter *handle)
@ -72,8 +83,11 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->init = &init; handle->init = &init;
handle->filter = &filter; handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data)); handle->localdata = pvPortMalloc(sizeof(struct data));
HomeLocationInitialize();
AttitudeStateInitialize(); AttitudeStateInitialize();
AltitudeFilterSettingsInitialize(); AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
reloadSettings = true;
return STACK_REQUIRED; return STACK_REQUIRED;
} }
@ -81,10 +95,10 @@ static int32_t init(stateFilter *self)
{ {
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
this->state[0] = 0.0f; this->altitudeState = 0.0f;
this->state[1] = 0.0f; this->velocityState = 0.0f;
this->state[2] = 0.0f; this->accelBiasState = 0.0f;
this->state[3] = 0.0f; this->accelState = 0.0f;
this->pos[0] = 0.0f; this->pos[0] = 0.0f;
this->pos[1] = 0.0f; this->pos[1] = 0.0f;
this->pos[2] = 0.0f; this->pos[2] = 0.0f;
@ -96,7 +110,7 @@ static int32_t init(stateFilter *self)
this->baroLast = 0.0f; this->baroLast = 0.0f;
this->accelLast = 0.0f; this->accelLast = 0.0f;
this->first_run = 1; this->first_run = 1;
AltitudeFilterSettingsGet(&this->settings); HomeLocationg_eGet(&this->gravity);
return 0; return 0;
} }
@ -104,10 +118,16 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
{ {
struct data *this = (struct data *)self->localdata; struct data *this = (struct data *)self->localdata;
if (reloadSettings) {
reloadSettings = false;
AltitudeFilterSettingsGet(&this->settings);
}
if (this->first_run) { if (this->first_run) {
// Initialize to current altitude reading at initial location // Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0; this->first_run = 0;
this->initTimer = xTaskGetTickCount();
} }
} else { } else {
// save existing position and velocity updates so GPS will still work // save existing position and velocity updates so GPS will still work
@ -115,13 +135,13 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
this->pos[0] = state->pos[0]; this->pos[0] = state->pos[0];
this->pos[1] = state->pos[1]; this->pos[1] = state->pos[1];
this->pos[2] = state->pos[2]; this->pos[2] = state->pos[2];
state->pos[2] = -this->state[0]; state->pos[2] = -this->altitudeState;
} }
if (IS_SET(state->updated, SENSORUPDATES_vel)) { if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->vel[0] = state->vel[0]; this->vel[0] = state->vel[0];
this->vel[1] = state->vel[1]; this->vel[1] = state->vel[1];
this->vel[2] = state->vel[2]; this->vel[2] = state->vel[2];
state->vel[2] = -this->state[1]; state->vel[2] = -this->velocityState;
} }
if (IS_SET(state->updated, SENSORUPDATES_accel)) { if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame // rotate accels into global coordinate frame
@ -129,54 +149,57 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
AttitudeStateGet(&att); AttitudeStateGet(&att);
float Rbe[3][3]; float Rbe[3][3];
Quaternion2R(&att.q1, Rbe); Quaternion2R(&att.q1, Rbe);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + 9.81f); float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
// low pass filter accelerometers // 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;
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
// correct accel offset (low pass zeroing) // allow the offset to reach quickly the target value in case of small AccelDriftKi
this->state[2] = (1.0f - this->settings.AccelDriftKi) * this->state[2] + this->settings.AccelDriftKi * this->state[3]; 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;
}
// correct velocity and position state (integration) // correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler // low pass for average dT, compensate timing jitter from scheduler
// //
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config); 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->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
this->accelLast = this->state[3] - this->state[2]; 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[0] = this->pos[0];
state->pos[1] = this->pos[1]; state->pos[1] = this->pos[1];
state->pos[2] = -this->state[0]; state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos; state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0]; state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1]; state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1]; state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel; state->updated |= SENSORUPDATES_vel;
} }
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass) // 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) // correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler // low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config); 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]; this->baroLast = state->baro[0];
state->pos[0] = this->pos[0]; state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1]; state->pos[1] = this->pos[1];
state->pos[2] = -this->state[0]; state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos; state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0]; state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1]; state->vel[1] = this->vel[1];
state->vel[2] = -this->state[1]; state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel; state->updated |= SENSORUPDATES_vel;
} }
} }
@ -184,6 +207,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
return FILTERRESULT_OK; return FILTERRESULT_OK;
} }
void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev->obj == AltitudeFilterSettingsHandle()) {
reloadSettings = true;
}
}
/** /**
* @} * @}