rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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

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

@ -106,7 +106,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
this->baroLast = state->baro[0];
}
} else {
// save existing position and velocity updates so GPS will still work
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->pos[0] = state->pos[0];
@ -121,7 +120,6 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->vel[2] = -this->state[1];
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
AttitudeStateData att;
AttitudeStateGet(&att);
@ -154,13 +152,14 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->updated |= SENSORUPDATES_vel;
}
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 velocity state (low pass differentiation)
float dT = PIOS_DELAY_DiffuS(this->baroLastTime) / 1.0e6f;
if (dT<0.001f) dT = 0.001f;
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];