Fixed calibration routines to ignore previous offsets during calibration, added scale compensation for MPU-6000

3 files changed, 155 insertions, 38 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 022003ee5..d8e871eb0 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -74,6 +74,16 @@
  
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+
+
+
 
 #include <arch/board/up_cpuload.h>
 extern struct system_load_s system_load;
@@ -294,7 +304,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		mag_minima[2][i] = FLT_MAX;
 	}
 
-	mavlink_log_info(mavlink_fd, "[commander] Please rotate in all directions.");
+	int fd = open(MAG_DEVICE_PATH, 0);
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null))
+		warn("WARNING: failed to set scale / offsets for mag");
+	close(fd);
+
+	mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
 	
 	uint64_t calibration_start = hrt_absolute_time();
 	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
@@ -443,6 +466,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 	}
 
+	fd = open(MAG_DEVICE_PATH, 0);
+	struct mag_scale mscale = {
+		mag_offset[0],
+		1.0f,
+		mag_offset[1],
+		1.0f,
+		mag_offset[2],
+		1.0f,
+	};
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+		warn("WARNING: failed to set scale / offsets for mag");
+	close(fd);
+
 	free(mag_maxima[0]);
 	free(mag_maxima[1]);
 	free(mag_maxima[2]);
@@ -468,6 +504,20 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 	int calibration_counter = 0;
 	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
 
+	/* set offsets to zero */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	struct gyro_scale gscale_null = { 
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
+		warn("WARNING: failed to set scale / offsets for gyro");
+	close(fd);
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -502,6 +552,20 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 		mavlink_log_critical(mavlink_fd, "[commander] Setting Z gyro offset failed!");
 	}
 
+	/* set offsets to actual value */
+	fd = open(GYRO_DEVICE_PATH, 0);
+	struct gyro_scale gscale = { 
+		gyro_offset[0],
+		1.0f,
+		gyro_offset[1],
+		1.0f,
+		gyro_offset[2],
+		1.0f,
+	};
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+		warn("WARNING: failed to set scale / offsets for gyro");
+	close(fd);
+
 	/* exit to gyro calibration mode */
 	status->flag_preflight_gyro_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -531,6 +595,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	int calibration_counter = 0;
 	float accel_offset[3] = {0.0f, 0.0f, 0.0f};
 
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	struct accel_scale ascale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
+		warn("WARNING: failed to set scale / offsets for accel");
+	close(fd);
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -556,20 +633,48 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
 	/* add the removed length from x / y to z, since we induce a scaling issue else */
 	float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]);
 
+	/* if length is correct, zero results here */
 	accel_offset[2] = accel_offset[2] + total_len;
 
-	if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) {
+	float scale = 9.80665f / total_len;
+
+	if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))) {
 		mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
 	}
 	
-	if (param_set(param_find("SENSOR_ACC_YOFF"), &(accel_offset[1]))) {
+	if (param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))) {
 		mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
 	}
 
-	if (param_set(param_find("SENSOR_ACC_ZOFF"), &(accel_offset[2]))) {
+	if (param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))) {
 		mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
 	}
 
+	if (param_set(param_find("SENS_ACC_XSCALE"), &(scale))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!");
+	}
+	
+	if (param_set(param_find("SENS_ACC_YSCALE"), &(scale))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting Y accel offset failed!");
+	}
+
+	if (param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
+		mavlink_log_critical(mavlink_fd, "[commander] Setting Z accel offset failed!");
+	}
+
+	fd = open(ACCEL_DEVICE_PATH, 0);
+	struct accel_scale ascale = {
+		accel_offset[0],
+		scale,
+		accel_offset[1],
+		scale,
+		accel_offset[2],
+		scale,
+	};
+	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+		warn("WARNING: failed to set scale / offsets for accel");
+	close(fd);
+
 	/* exit to gyro calibration mode */
 	status->flag_preflight_accel_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 6e36002dd..fcd0608a4 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -44,17 +44,21 @@
 
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
-PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
 
-PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
-PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
+
+PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 0.0f);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index d388a6d5a..7c6437608 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -182,7 +182,8 @@ private:
 
 		float gyro_offset[3];
 		float mag_offset[3];
-		float acc_offset[3];
+		float accel_offset[3];
+		float accel_scale[3];
 
 		int rc_type;
 
@@ -203,8 +204,9 @@ private:
 		param_t rc_type;
 
 		param_t gyro_offset[3];
+		param_t accel_offset[3];
+		param_t accel_scale[3];
 		param_t mag_offset[3];
-		param_t acc_offset[3];
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -383,19 +385,22 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
 
 	/* gyro offsets */
-	_parameter_handles.gyro_offset[0] = param_find("SENSOR_GYRO_XOFF");
-	_parameter_handles.gyro_offset[1] = param_find("SENSOR_GYRO_YOFF");
-	_parameter_handles.gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF");
+	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
+	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
+	_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
 
 	/* accel offsets */
-	_parameter_handles.acc_offset[0] = param_find("SENSOR_ACC_XOFF");
-	_parameter_handles.acc_offset[1] = param_find("SENSOR_ACC_YOFF");
-	_parameter_handles.acc_offset[2] = param_find("SENSOR_ACC_ZOFF");
+	_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
+	_parameter_handles.accel_offset[1] = param_find("SENS_ACC_YOFF");
+	_parameter_handles.accel_offset[2] = param_find("SENS_ACC_ZOFF");
+	_parameter_handles.accel_scale[0] = param_find("SENS_ACC_XSCALE");
+	_parameter_handles.accel_scale[1] = param_find("SENS_ACC_YSCALE");
+	_parameter_handles.accel_scale[2] = param_find("SENS_ACC_ZSCALE");
 
 	/* mag offsets */
-	_parameter_handles.mag_offset[0] = param_find("SENSOR_MAG_XOFF");
-	_parameter_handles.mag_offset[1] = param_find("SENSOR_MAG_YOFF");
-	_parameter_handles.mag_offset[2] = param_find("SENSOR_MAG_ZOFF");
+	_parameter_handles.mag_offset[0] = param_find("SENS_MAG_XOFF");
+	_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
+	_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
@@ -493,9 +498,12 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
 
 	/* accel offsets */
-	param_get(_parameter_handles.acc_offset[0], &(_parameters.acc_offset[0]));
-	param_get(_parameter_handles.acc_offset[1], &(_parameters.acc_offset[1]));
-	param_get(_parameter_handles.acc_offset[2], &(_parameters.acc_offset[2]));
+	param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
+	param_get(_parameter_handles.accel_offset[1], &(_parameters.accel_offset[1]));
+	param_get(_parameter_handles.accel_offset[2], &(_parameters.accel_offset[2]));
+	param_get(_parameter_handles.accel_scale[0], &(_parameters.accel_scale[0]));
+	param_get(_parameter_handles.accel_scale[1], &(_parameters.accel_scale[1]));
+	param_get(_parameter_handles.accel_scale[2], &(_parameters.accel_scale[2]));
 
 	/* mag offsets */
 	param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
@@ -643,9 +651,9 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		const float range_g = 4.0f;
 		/* scale from 14 bit to m/s2 */
-		accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
-		accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
-		accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		accel_report.x = (((accel_report.x_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		accel_report.y = (((accel_report.y_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
+		accel_report.z = (((accel_report.z_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
 		raw.accelerometer_counter++;
 
 	} else {
@@ -822,12 +830,12 @@ Sensors::parameter_update_poll(bool forced)
 
 		fd = open(ACCEL_DEVICE_PATH, 0);
 		struct accel_scale ascale = {
-			_parameters.acc_offset[0],
-			1.0f,
-			_parameters.acc_offset[1],
-			1.0f,
-			_parameters.acc_offset[2],
-			1.0f,
+			_parameters.accel_offset[0],
+			_parameters.accel_scale[0],
+			_parameters.accel_offset[1],
+			_parameters.accel_scale[1],
+			_parameters.accel_offset[2],
+			_parameters.accel_scale[2],
 		};
 		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
 			warn("WARNING: failed to set scale / offsets for accel");
@@ -1004,7 +1012,7 @@ Sensors::task_main()
 
 	parameter_update_poll(true /* forced */);
 
-	/* advertise the sensor_combined topic and make the initial publication */
+	/* advertise the SENS_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
 
 	/* advertise the manual_control topic */
@@ -1092,7 +1100,7 @@ Sensors::start()
 	ASSERT(_sensors_task == -1);
 
 	/* start the task */
-	_sensors_task = task_create("sensor_task",
+	_sensors_task = task_create("SENS_task",
 				    SCHED_PRIORITY_MAX - 5,
 				    4096,	/* XXX may be excesssive */
 				    (main_t)&Sensors::task_main_trampoline,