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

Merge commit '84c3705390a072d92bf69b58ae4d72a09989290e' into portugal_backports

Browse Source
This commit is contained in:
Corvus Corax 2012-06-01 13:06:44 +02:00
commit fed9d98824
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -88,6 +88,8 @@ 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 gyro_scale[3] = {0,0,0};
static float R[3][3] = {{0}};
static int8_t rotate = 0;
@ -361,9 +363,9 @@ static void SensorsTask(void *parameters)
float gyros[3] = {(float) gyro_accum[1] / gyro_samples,
(float) gyro_accum[0] / gyro_samples,
-(float) gyro_accum[2] / gyro_samples};
float gyros_out[3] = {gyros[0] * gyro_scaling,
gyros[1] * gyro_scaling,
gyros[2] * gyro_scaling};
float gyros_out[3] = {gyros[0] * gyro_scaling * gyro_scale[0] - gyro_bias[0],
gyros[1] * gyro_scaling * gyro_scale[1] - gyro_bias[1],
gyros[2] * gyro_scaling * gyro_scale[2] - gyro_bias[2]};
if (rotate) {
rot_mult(R, gyros_out, gyros);
gyrosData.x = gyros[0];
@ -448,6 +450,12 @@ 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];
gyro_scale[0] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_X];
gyro_scale[1] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Y];
gyro_scale[2] = cal.gyro_scale[REVOCALIBRATION_GYRO_SCALE_Z];
AttitudeSettingsData attitudeSettings;
AttitudeSettingsGet(&attitudeSettings);