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 <attitudestate.h>
#include <altitudefiltersettings.h>
#include <homelocation.h>
#include <CoordinateConversions.h>
// Private constants
#define STACK_REQUIRED 128
// duration of accel bias initialization phase
#define INITIALIZATION_DURATION_MS 5000
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
#define STACK_REQUIRED 128
#define DT_ALPHA 1e-2f
#define DT_MIN 1e-6f
#define DT_MAX 1.0f
#define DT_AVERAGE 1e-3f
static volatile bool reloadSettings;
// 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
@ -56,7 +64,9 @@ struct data {
float accelLast;
float baroLast;
bool first_run;
portTickType initTimer;
AltitudeFilterSettingsData settings;
float gravity;
};
// Private variables
@ -65,6 +75,7 @@ struct data {
static int32_t init(stateFilter *self);
static filterResult filter(stateFilter *self, stateEstimation *state);
static void settingsUpdatedCb(UAVObjEvent *ev);
int32_t filterAltitudeInitialize(stateFilter *handle)
@ -72,8 +83,11 @@ int32_t filterAltitudeInitialize(stateFilter *handle)
handle->init = &init;
handle->filter = &filter;
handle->localdata = pvPortMalloc(sizeof(struct data));
HomeLocationInitialize();
AttitudeStateInitialize();
AltitudeFilterSettingsInitialize();
AltitudeFilterSettingsConnectCallback(&settingsUpdatedCb);
reloadSettings = true;
return STACK_REQUIRED;
}
@ -81,10 +95,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;
@ -96,7 +110,7 @@ static int32_t init(stateFilter *self)
this->baroLast = 0.0f;
this->accelLast = 0.0f;
this->first_run = 1;
AltitudeFilterSettingsGet(&this->settings);
HomeLocationg_eGet(&this->gravity);
return 0;
}
@ -104,10 +118,16 @@ static filterResult 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)) {
this->first_run = 0;
this->initTimer = xTaskGetTickCount();
}
} else {
// 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[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
@ -129,54 +149,57 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
AttitudeStateGet(&att);
float Rbe[3][3];
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
this->state[3] = (1.0f - this->settings.AccelLowPassKp) * this->state[3] + 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->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
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 - 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)
// 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;
}
}
@ -184,6 +207,12 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
return FILTERRESULT_OK;
}
void settingsUpdatedCb(UAVObjEvent *ev)
{
if (ev->obj == AltitudeFilterSettingsHandle()) {
reloadSettings = true;
}
}
/**
* @}