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

OP-975/OP-1068 replace simple scaling with a calibration matrix for magnetometer correction

Browse Source
This commit is contained in:
Alessio Morale 2013-07-17 13:28:55 +00:00
parent c6b5011cec
commit 8eeb3217bf
2 changed files with 50 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
flight/modules/Sensors/sensors.c
View File

@ -84,10 +84,11 @@ AccelGyroSettingsData agcal;
// These values are initialized by settings but can be updated by the attitude algorithm
static float mag_bias[3] = { 0, 0, 0 };
static float mag_scale[3] = { 0, 0, 0 };
static float mag_transform[3][3]={{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1}};
// temp coefficient to calculate gyro bias
static volatile bool gyro_temp_calibrated = false;
static volatile bool accel_temp_calibrated = false;
static float R[3][3] = {
{ 0 }
};
@ -119,6 +120,7 @@ int32_t SensorsInitialize(void)
RevoCalibrationConnectCallback(&settingsUpdatedCb);
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
@ -359,7 +361,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
accels_out[1] -= agcal.accel_temp_coeff.Y * ctemp;
accels_out[2] -= agcal.accel_temp_coeff.Z * ctemp;
}
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -389,7 +390,6 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
gyros_out[1] -= (agcal.gyro_temp_coeff.Y + agcal.gyro_temp_coeff.Y2 * ctemp) * ctemp;
gyros_out[2] -= (agcal.gyro_temp_coeff.Z + agcal.gyro_temp_coeff.Z2 * ctemp) * ctemp;
}
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -411,20 +411,16 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) {
int16_t values[3];
PIOS_HMC5883_ReadMag(values);
float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0],
(float)values[0] * mag_scale[1] - mag_bias[1],
-(float)values[2] * mag_scale[2] - mag_bias[2] };
if (rotate) {
float mag_out[3];
rot_mult(R, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
} else {
mag.x = mags[0];
mag.y = mags[1];
mag.z = mags[2];
}
float mags[3] = { (float)values[1] - mag_bias[0],
(float)values[0] - mag_bias[1],
-(float)values[2] - mag_bias[2] };
float mag_out[3];
rot_mult(mag_transform, mags, mag_out);
mag.x = mag_out[0];
mag.y = mag_out[1];
mag.z = mag_out[2];
MagSensorSet(&mag);
mag_update_time = PIOS_DELAY_GetRaw();
@ -449,9 +445,6 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
mag_bias[0] = cal.mag_bias.X;
mag_bias[1] = cal.mag_bias.Y;
mag_bias[2] = cal.mag_bias.Z;
mag_scale[0] = cal.mag_scale.X;
mag_scale[1] = cal.mag_scale.Y;
mag_scale[2] = cal.mag_scale.Z;
accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) &&
(fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f);
@ -469,14 +462,44 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
attitudeSettings.BoardRotation.Yaw == 0) {
rotate = 0;
} else {
float rotationQuat[4];
rotate = 1;
}
float rotationQuat[4];
const float rpy[3] = { attitudeSettings.BoardRotation.Roll,
attitudeSettings.BoardRotation.Pitch,
attitudeSettings.BoardRotation.Yaw };
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
rotate = 1;
}
RPY2Quaternion(rpy, rotationQuat);
Quaternion2R(rotationQuat, R);
mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
R[1][0] * cal.mag_transform.r0c1 +
R[2][0] * cal.mag_transform.r0c2;
mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
R[1][1] * cal.mag_transform.r0c1 +
R[2][1] * cal.mag_transform.r0c2;
mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
R[1][2] * cal.mag_transform.r0c1 +
R[2][2] * cal.mag_transform.r0c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
R[1][0] * cal.mag_transform.r1c1 +
R[2][0] * cal.mag_transform.r1c2;
mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
R[1][1] * cal.mag_transform.r1c1 +
R[2][1] * cal.mag_transform.r1c2;
mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
R[1][2] * cal.mag_transform.r1c1 +
R[2][2] * cal.mag_transform.r1c2;
mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
R[1][0] * cal.mag_transform.r2c1 +
R[2][0] * cal.mag_transform.r2c2;
mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
R[1][1] * cal.mag_transform.r2c1 +
R[2][1] * cal.mag_transform.r2c2;
mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
R[1][2] * cal.mag_transform.r2c1 +
R[2][2] * cal.mag_transform.r2c2;
}
/**
* @}

3
shared/uavobjectdefinition/revocalibration.xml
View File

@ -2,7 +2,8 @@
<object name="RevoCalibration" singleinstance="true" settings="true" category="Sensors">
<description>Settings for the INS to control the algorithm and what is updated</description>
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
<field name="mag_transform" units="gain" type="float" elementnames="r0c0,r0c1,r0c2,r1c0,r1c1,r1c2,r2c0,r2c1,r2c2"
defaultvalue="1,0,0,0,1,0,0,0,1"/>
<!-- These settings are related to how the sensors are post processed -->
<!-- TODO: reimplement, put elsewhere (later) -->
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>