From bce043a21b7f39f786755fa3118c2c5e25eb8a94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Aug 2012 17:37:58 +0200 Subject: Fixed mag axis assignment, fixed mag calibration --- apps/commander/commander.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) (limited to 'apps/commander/commander.c') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e290c2140..47829ddf5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -376,9 +376,22 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]); float mag_offset[3]; - mag_offset[0] = (max_avg[0] - min_avg[0]); - mag_offset[1] = (max_avg[1] - min_avg[1]); - mag_offset[2] = (max_avg[2] - min_avg[2]); + + /** + * The offset is subtracted from the sensor values, so the result is the + * POSITIVE number that has to be subtracted from the sensor data + * to shift the center to zero + * + * offset = max - ((max - min) / 2.0f) + * + * which reduces to + * + * offset = (max + min) / 2.0f + */ + + mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; + mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; + mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET] = mag_offset[0]; global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET] = mag_offset[1]; -- cgit v1.2.3