mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-28 17:54:15 +01:00
fixes to INS backported from portugal flight branch
This commit is contained in:
parent
a786c6e6b9
commit
c9d41e2a23
@ -659,9 +659,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
|
||||
|
||||
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};
|
||||
INSStatePrediction(gyros, &accelsData.x, dT);
|
||||
|
||||
AttitudeActualData attitude;
|
||||
@ -797,7 +797,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
velocityActual.Down = Nav.Vel[2];
|
||||
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};
|
||||
INSSetGyroBias(zeros);
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user