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

EKF gyro bias into the object now working propely

Browse Source
This commit is contained in:
James Cotton 2012-07-21 15:11:04 -05:00
parent 02dfa7bd82
commit 3e33bb49a2
2 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/Libraries/insgps13state.c
View File

@ -359,6 +359,9 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
// ************* CovariancePrediction *************

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

@ -601,7 +601,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings
INSSetGyroBias(&gyrosBias.x);
float gyro_bias[3] = {-gyrosBias.x * F_PI / 180.0f, -gyrosBias.y * F_PI / 180.0f, -gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
@ -635,7 +636,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
INSSetGyroBias(&gyrosBias.x);
float gyro_bias[3] = {-gyrosBias.x * F_PI / 180.0f, -gyrosBias.y * F_PI / 180.0f, -gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
@ -708,7 +710,8 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
INSSetGyroBias(&gyrosBias.x);
float gyro_bias[3] = {-gyrosBias.x * F_PI / 180.0f, -gyrosBias.y * F_PI / 180.0f, -gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
@ -734,16 +737,6 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
if (revoCalibration.BiasCorrectedRaw && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
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
INSCovariancePrediction(dT);
@ -819,10 +812,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
velocityActual.East = Nav.Vel[1];
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) {
float zeros[3] = {0.0f,0.0f,0.0f};
INSSetGyroBias(zeros);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
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);
}
return 0;