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

Make the sensors code only apply the GyroBias from the UAVO which is now coming

Browse Source 
consistently only from the attitude module
This commit is contained in:
James Cotton 2012-07-21 13:51:20 -05:00
parent e5bd999975
commit 7cdf47c1d4
2 changed files with 38 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -367,7 +367,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, home.Be, brot);
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
@ -571,6 +571,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
BaroAltitudeGet(&baroData);
GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
HomeLocationGet(&home);
// Discard mag if it has NAN (normally from bad calibration)
@ -604,6 +605,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings
INSSetGyroBias(&gyrosBias.x);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
@ -635,6 +639,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
INSSetMagNorth(home.Be);
// Initialize the gyro bias from the settings
INSSetGyroBias(&gyrosBias.x);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
@ -703,12 +710,21 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
INSSetGyroBias(&gyrosBias.x);
gyroBiasSettingsUpdated = false;
}
// 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};
// back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
if (revoCalibration.BiasCorrectedRaw) {
gyros[0] -= gyrosBias.x * F_PI / 180.0f;
gyros[1] -= gyrosBias.y * F_PI / 180.0f;
gyros[2] -= gyrosBias.z * F_PI / 180.0f;
}
// Advance the state estimate
INSStatePrediction(gyros, &accelsData.x, dT);
@ -723,11 +739,15 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
// Copy the gyro bias into the UAVO
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);
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);
@ -739,7 +759,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
sensors |= BARO_SENSOR;
INSSetMagNorth(home.Be);
if (gps_updated && outdoor_mode)
{
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);

14
flight/Modules/Sensors/sensors.c
View File

@ -88,7 +88,6 @@ static float mag_bias[3] = {0,0,0};
static float mag_scale[3] = {0,0,0};
static float accel_bias[3] = {0,0,0};
static float accel_scale[3] = {0,0,0};
static float gyro_bias[3] = {0,0,0};
static float R[3][3] = {{0}};
static int8_t rotate = 0;
@ -377,12 +376,12 @@ static void SensorsTask(void *parameters)
}
if (bias_correct_gyro) {
// Apply bias correction to the gyros
// Apply bias correction to the gyros from the state estimator
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosData.x += gyrosBias.x - gyro_bias[0];
gyrosData.y += gyrosBias.y - gyro_bias[1];
gyrosData.z += gyrosBias.z - gyro_bias[2];
gyrosData.x += gyrosBias.x;
gyrosData.y += gyrosBias.y;
gyrosData.z += gyrosBias.z;
}
GyrosSet(&gyrosData);
@ -449,9 +448,8 @@ static void settingsUpdatedCb(UAVObjEvent * objEv) {
accel_scale[0] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_X];
accel_scale[1] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Y];
accel_scale[2] = cal.accel_scale[REVOCALIBRATION_ACCEL_SCALE_Z];
gyro_bias[0] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_X];
gyro_bias[1] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Y];
gyro_bias[2] = cal.gyro_bias[REVOCALIBRATION_GYRO_BIAS_Z];
// Do not store gyros_bias here as that comes from the state estimator and should be
// used from GyroBias directly
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);