rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

OP-1235 gives better names to state variables

Browse Source 
Conflicts:
	flight/modules/StateEstimation/filteraltitude.c
This commit is contained in:
Alessio Morale 2014-02-23 13:04:39 +01:00
parent 14f2894464
commit bc3636bbd6
1 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
}
}