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

filterekf gyro bias adjustment bugfix

Browse Source
This commit is contained in:
Corvus Corax 2013-05-25 16:18:18 +02:00
parent 6c25e82633
commit 7bf58f09c5
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/modules/StateEstimation/filterekf.c
View File

@ -298,9 +298,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] += Nav.gyro_bias[0];
state->gyro[1] += Nav.gyro_bias[1];
state->gyro[2] += Nav.gyro_bias[2];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];
@ -333,9 +333,9 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
state->attitude[1] = Nav.q[1];
state->attitude[2] = Nav.q[2];
state->attitude[3] = Nav.q[3];
state->gyro[0] -= Nav.gyro_bias[0];
state->gyro[1] -= Nav.gyro_bias[1];
state->gyro[2] -= Nav.gyro_bias[2];
state->gyro[0] -= RAD2DEG(Nav.gyro_bias[0]);
state->gyro[1] -= RAD2DEG(Nav.gyro_bias[1]);
state->gyro[2] -= RAD2DEG(Nav.gyro_bias[2]);
state->pos[0] = Nav.Pos[0];
state->pos[1] = Nav.Pos[1];
state->pos[2] = Nav.Pos[2];