rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Fix from Corvus for gyro bias from EKF

Browse Source
This commit is contained in:
James Cotton 2012-04-14 12:09:12 -05:00
parent 414e62f14e
commit 5d160860a3
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/Attitude/revolution/attitude.c
View File

@ -617,9 +617,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f,
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f,
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f,
(gyrosData.y - gyrosBias.y) * F_PI / 180.0f,
(gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
// Advance the state estimate
INSStatePrediction(gyros, &accelsData.x, dT);
@ -635,9 +635,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
AttitudeActualSet(&attitude);
// Copy the gyro bias into the UAVO
gyrosBias.x = Nav.gyro_bias[0];
gyrosBias.y = Nav.gyro_bias[1];
gyrosBias.z = Nav.gyro_bias[2];
gyrosBias.x = -Nav.gyro_bias[0] * 180.0f / F_PI;
gyrosBias.y = -Nav.gyro_bias[1] * 180.0f / F_PI;
gyrosBias.z = -Nav.gyro_bias[2] * 180.0f / F_PI;
GyrosBiasSet(&gyrosBias);
// Advance the covariance estimate