rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

some uncrustification

Browse Source
This commit is contained in:
Corvus Corax 2013-06-28 19:52:09 +02:00
parent b9038e5ed6
commit 2fb0c0a3ce
1 changed files with 76 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -41,18 +41,18 @@
#define BARO_SENSOR_KP 0.05f #define BARO_SENSOR_KP 0.05f
#define ACCEL_DRIFT_KP 0.001f #define ACCEL_DRIFT_KP 0.001f
#define DT_ALPHA 1e-3f #define DT_ALPHA 1e-3f
// Private types // Private types
struct data { struct data {
float state[3]; // state = altitude,velocity,accel_offset float state[3]; // state = altitude,velocity,accel_offset
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
float dTA; float dTA;
int32_t lastTime; int32_t lastTime;
float baroLast; float baroLast;
int32_t baroLastTime; int32_t baroLastTime;
bool first_run; bool first_run;
}; };
// Private variables // Private variables
@ -76,17 +76,17 @@ 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->state[0] = 0.0f;
this->state[1] = 0.0f; this->state[1] = 0.0f;
this->state[2] = 0.0f; this->state[2] = 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;
this->vel[0] = 0.0f; this->vel[0] = 0.0f;
this->vel[1] = 0.0f; this->vel[1] = 0.0f;
this->vel[2] = 0.0f; this->vel[2] = 0.0f;
this->dTA = -1; this->dTA = -1;
this->first_run = 1; this->first_run = 1;
return 0; return 0;
} }
@ -97,83 +97,82 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
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_accel)) { if (IS_SET(state->updated, SENSORUPDATES_accel)) {
this->lastTime = PIOS_DELAY_GetRaw(); this->lastTime = PIOS_DELAY_GetRaw();
} }
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0; this->first_run = 0;
this->baroLastTime = PIOS_DELAY_GetRaw(); this->baroLastTime = PIOS_DELAY_GetRaw();
this->state[0] = state->baro[0]; this->state[0] = state->baro[0];
this->baroLast = state->baro[0]; this->baroLast = state->baro[0];
} }
} else { } else {
// save existing position and velocity updates so GPS will still work // save existing position and velocity updates so GPS will still work
if (IS_SET(state->updated, SENSORUPDATES_pos)) { if (IS_SET(state->updated, SENSORUPDATES_pos)) {
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->state[0];
} }
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->state[1];
} }
if(IS_SET(state->updated, SENSORUPDATES_accel)) { if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
AttitudeStateData att;
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);
// rotate accels into global coordinate frame // correct accel offset (low pass zeroing)
AttitudeStateData att;
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);
// correct accel offset (low pass zeroing)
this->state[2] = (1.0f - ACCEL_DRIFT_KP) * this->state[2] + ACCEL_DRIFT_KP * current; this->state[2] = (1.0f - ACCEL_DRIFT_KP) * this->state[2] + ACCEL_DRIFT_KP * current;
// 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_DELAY_DiffuS(this->lastTime) / 1.0e6f; float dT = PIOS_DELAY_DiffuS(this->lastTime) / 1.0e6f;
if (this->dTA<0) { if (this->dTA < 0) {
this->dTA = dT; this->dTA = dT;
} else { } else {
this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA; this->dTA = this->dTA * (1.0f - DT_ALPHA) + dT * DT_ALPHA;
} }
this->state[1] += (current - this->state[1]) * this->dTA; this->state[1] += (current - this->state[1]) * this->dTA;
this->state[0] += this->state[1] * this->dTA; this->state[0] += this->state[1] * this->dTA;
state->pos[0] = this->pos[0]; state->pos[0] = this->pos[0];
state->pos[1] = this->pos[2]; state->pos[1] = this->pos[2];
state->pos[2] = -this->state[0]; state->pos[2] = -this->state[0];
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[2]; state->vel[1] = this->vel[2];
state->vel[2] = -this->state[0]; state->vel[2] = -this->state[0];
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)
this->state[0] = (1.0f - BARO_SENSOR_KP) * this->state[0] + BARO_SENSOR_KP * state->baro[0];
// correct the altitude state (simple low pass) // correct the velocity state (low pass differentiation)
this->state[0] = (1.0f - BARO_SENSOR_KP) * this->state[0] + BARO_SENSOR_KP * state->baro[0]; float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
if (dT < 0.001f) {
dT = 0.001f;
}
this->state[1] = (1.0f - BARO_SENSOR_KP) * this->state[1] + BARO_SENSOR_KP * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
// correct the velocity state (low pass differentiation) state->pos[0] = this->pos[0];
float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f; state->pos[1] = this->pos[2];
if (dT<0.001f) dT = 0.001f; state->pos[2] = -this->state[0];
this->state[1] = (1.0f - BARO_SENSOR_KP) * this->state[1] + BARO_SENSOR_KP * (state->baro[0] - this->baroLast) / dT; state->updated |= SENSORUPDATES_pos;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0]; state->vel[0] = this->vel[0];
state->pos[1] = this->pos[2]; state->vel[1] = this->vel[2];
state->pos[2] = -this->state[0]; state->vel[2] = -this->state[0];
state->updated |= SENSORUPDATES_pos; state->updated |= SENSORUPDATES_vel;
}
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[2];
state->vel[2] = -this->state[0];
state->updated |= SENSORUPDATES_vel;
}
} }
return 0; return 0;