From 84c3705390a072d92bf69b58ae4d72a09989290e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sat, 14 Apr 2012 14:01:28 +0200 Subject: [PATCH 1/2] use gyro scale and bias in sensor module --- flight/Modules/Sensors/sensors.c | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 2e097d0c6..a71b9e72a 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -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; @@ -359,9 +361,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]; @@ -455,6 +457,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); From 2d3645cb0adfa111c9d5b73bcdab7c72775de993 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 17 Apr 2012 11:06:08 +0200 Subject: [PATCH 2/2] ManualControl: Do not process input channels at all if theres invalid values to prevent flightmode changes due to glitch --- flight/Modules/ManualControl/manualcontrol.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 1b74e096c..ae5fb899d 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -63,7 +63,7 @@ #define ARMED_TIME_MS 1000 #define ARMED_THRESHOLD 0.50f //safe band to allow a bit of calibration error or trim offset (in microseconds) -#define CONNECTION_OFFSET 150 +#define CONNECTION_OFFSET 250 // Private types typedef enum @@ -314,7 +314,7 @@ static void manualControlTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } - } else { + } else if (valid_input_detected) { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range