Fixed mag axis assignment, fixed mag calibration

4 files changed, 22 insertions, 9 deletions

diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile
index 2038e982b..a6d471567 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 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];
diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile
index dadc2993d..125839bb3 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 5e93f180f..7933e8518 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;