From bce043a21b7f39f786755fa3118c2c5e25eb8a94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Aug 2012 17:37:58 +0200 Subject: [PATCH] Fixed mag axis assignment, fixed mag calibration --- apps/ardrone_control/Makefile | 2 +- apps/commander/commander.c | 19 ++++++++++++++++--- apps/sensors/Makefile | 2 +- apps/sensors/sensors.c | 8 ++++---- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile index 2038e982b5..a6d471567b 100644 --- a/apps/ardrone_control/Makefile +++ b/apps/ardrone_control/Makefile @@ -37,7 +37,7 @@ APPNAME = ardrone_control PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 3096 +STACKSIZE = 4096 # explicit list of sources - not everything is built currently CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e290c2140c..47829ddf50 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]; diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index dadc2993d1..125839bb35 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2048 +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index 5e93f180f1..7933e8518d 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -803,7 +803,7 @@ int sensors_main(int argc, char *argv[]) /* assign negated value, except for -SHORT_MAX, as it would wrap there */ raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor - raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32768 : buf_accelerometer[2]; // z of the board is z of the sensor // XXX read range from sensor float range_g = 4.0f; @@ -820,9 +820,9 @@ int sensors_main(int argc, char *argv[]) /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : -buf_magnetometer[1]; // x of the board is -y of the sensor - raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? -32767 : buf_magnetometer[0]; // y on the board is x of the sensor - raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32767 : buf_magnetometer[2]; // z of the board is z of the sensor + raw.magnetometer_raw[0] = (buf_magnetometer[1] == -32768) ? 32767 : buf_magnetometer[1]; // x of the board is -y of the sensor + raw.magnetometer_raw[1] = (buf_magnetometer[0] == -32768) ? 32767 : -buf_magnetometer[0]; // y on the board is x of the sensor + raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f;