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

fixes to INS backported from portugal flight branch

Browse Source
This commit is contained in:
Corvus Corax 2012-06-01 13:02:26 +02:00
parent a786c6e6b9
commit c9d41e2a23
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/Modules/Attitude/revolution/attitude.c
View File

@ -659,9 +659,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias); GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, float gyros[3] = {(gyrosData.x - gyrosBias.x) * F_PI / 180.0f,
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.y - gyrosBias.y) * F_PI / 180.0f,
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; (gyrosData.z - gyrosBias.z) * F_PI / 180.0f};
INSStatePrediction(gyros, &accelsData.x, dT); INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude; AttitudeActualData attitude;
@ -797,7 +797,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
velocityActual.Down = Nav.Vel[2]; velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual); VelocityActualSet(&velocityActual);
if(fabs(Nav.gyro_bias[0]) > 0.1f || fabs(Nav.gyro_bias[1]) > 0.1f || fabs(Nav.gyro_bias[2]) > 0.1f) { if(fabs(Nav.gyro_bias[0]) > 0.5f || fabs(Nav.gyro_bias[1]) > 0.5f || fabs(Nav.gyro_bias[2]) > 0.5f) {
float zeros[3] = {0.0f,0.0f,0.0f}; float zeros[3] = {0.0f,0.0f,0.0f};
INSSetGyroBias(zeros); INSSetGyroBias(zeros);
} }