From a6b43ecef086aaacd0e830ab283311b499530612 Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Wed, 18 Mar 2015 04:43:30 +0100
Subject: [PATCH] OP-1793 - Apply accel bias before scaling
---
flight/modules/Sensors/sensors.c | 11 +++++++----
1 file changed, 7 insertions(+), 4 deletions(-)
diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c
index b1d8288d6..c45024d59 100644
--- a/flight/modules/Sensors/sensors.c
+++ b/flight/modules/Sensors/sensors.c
@@ -411,9 +411,9 @@ static void handleAccel(float *samples, float temperature)
AccelSensorData accelSensorData;
updateAccelTempBias(temperature);
- float accels_out[3] = { samples[0] * agcal.accel_scale.X - agcal.accel_bias.X - accel_temp_bias[0],
- samples[1] * agcal.accel_scale.Y - agcal.accel_bias.Y - accel_temp_bias[1],
- samples[2] * agcal.accel_scale.Z - agcal.accel_bias.Z - accel_temp_bias[2] };
+ float accels_out[3] = { (samples[0] - agcal.accel_bias.X) * agcal.accel_scale.X - accel_temp_bias[0],
+ (samples[1] - agcal.accel_bias.Y) * agcal.accel_scale.Y - accel_temp_bias[1],
+ (samples[2] - agcal.accel_bias.Z) * agcal.accel_scale.Z - accel_temp_bias[2] };
rot_mult(R, accels_out, samples);
accelSensorData.x = samples[0];
@@ -485,7 +485,10 @@ static void updateAccelTempBias(float temperature)
if ((accel_temp_calibrated) && !accel_temp_calibration_count) {
accel_temp_calibration_count = TEMP_CALIB_INTERVAL;
if (accel_temp_calibrated) {
- float ctemp = boundf(accel_temperature, agcal.temp_calibrated_extent.max, agcal.temp_calibrated_extent.min);
+ float ctemp = boundf(accel_temperature,
+ agcal.temp_calibrated_extent.max,
+ agcal.temp_calibrated_extent.min);
+
accel_temp_bias[0] = agcal.accel_temp_coeff.X * ctemp;
accel_temp_bias[1] = agcal.accel_temp_coeff.Y * ctemp;
accel_temp_bias[2] = agcal.accel_temp_coeff.Z * ctemp;