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

OP-1149 introduce a first order temperature calibration for gyro and accel.

Browse Source 
gyro Z is instead using a 2nd degree polinomial for better accuracy as
its drift is harder to be corrected as with the other twos.
This commit is contained in:
Alessio Morale 2013-12-23 11:26:10 +01:00
parent f95a86eaed
commit 558e2e1437
2 changed files with 32 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -87,6 +87,9 @@ static float accel_bias[3] = { 0, 0, 0 };
static float accel_scale[3] = { 0, 0, 0 };
static float gyro_staticbias[3] = { 0, 0, 0 };
static float gyro_scale[3] = { 0, 0, 0 };
// temp coefficient to calculate gyro bias
static float gyro_temp_coeff[4] = {0};
static float accel_temp_coeff[4] = {0};
static float R[3][3] = {
{ 0 }
@ -344,9 +347,15 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
float accels[3] = { (float)accel_accum[0] / accel_samples,
(float)accel_accum[1] / accel_samples,
(float)accel_accum[2] / accel_samples };
float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0] - accel_bias[0],
accels[1] * accel_scaling * accel_scale[1] - accel_bias[1],
accels[2] * accel_scaling * accel_scale[2] - accel_bias[2] };
float accels_out[3] = { accels[0] * accel_scaling * accel_scale[0]
- accel_bias[0]
- accel_temp_coeff[0] * accelSensorData.temperature,
accels[1] * accel_scaling * accel_scale[1]
- accel_bias[1]
- accel_temp_coeff[1] * accelSensorData.temperature,
accels[2] * accel_scaling * accel_scale[2]
- accel_bias[2]
- accel_temp_coeff[2] * accelSensorData.temperature};
if (rotate) {
rot_mult(R, accels_out, accels);
accelSensorData.x = accels[0];
@ -363,9 +372,16 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
float gyros[3] = { (float)gyro_accum[0] / gyro_samples,
(float)gyro_accum[1] / gyro_samples,
(float)gyro_accum[2] / gyro_samples };
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0] - gyro_staticbias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_staticbias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_staticbias[2] };
float gyros_out[3] = { gyros[0] * gyro_scaling * gyro_scale[0]
- gyro_staticbias[0]
- gyro_temp_coeff[0] * gyroSensorData.temperature,
gyros[1] * gyro_scaling * gyro_scale[1]
- gyro_staticbias[1]
- gyro_temp_coeff[1] * gyroSensorData.temperature,
gyros[2] * gyro_scaling * gyro_scale[2]
- gyro_staticbias[2]
- gyro_temp_coeff[2] * gyroSensorData.temperature
- gyro_temp_coeff[3] * gyroSensorData.temperature * gyroSensorData.temperature};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyroSensorData.x = gyros[0];
@ -440,7 +456,13 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
gyro_scale[0] = cal.gyro_scale.X;
gyro_scale[1] = cal.gyro_scale.Y;
gyro_scale[2] = cal.gyro_scale.Z;
gyro_temp_coeff[0] = cal.gyro_temp_coeff.X;
gyro_temp_coeff[1] = cal.gyro_temp_coeff.Y;
gyro_temp_coeff[2] = cal.gyro_temp_coeff.Z;
gyro_temp_coeff[3] = cal.gyro_temp_coeff.Z2;
accel_temp_coeff[0] = cal.accel_temp_coeff.X;
accel_temp_coeff[1] = cal.accel_temp_coeff.Y;
accel_temp_coeff[2] = cal.accel_temp_coeff.Z;
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);

4
shared/uavobjectdefinition/revocalibration.xml
View File

@ -6,11 +6,13 @@
<!-- Sensor calibration -->
<field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="accel_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
<field name="accel_temp_coeff" units="" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
<field name="gyro_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
<field name="gyro_temp_coeff" units="" type="float" elementnames="X,Y,Z,Z2" defaultvalue="0"/>
<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"/>
<!-- 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"/>