Calibration WIP, not compiling

5 files changed, 192 insertions, 213 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 43df6ceb2..f11c583d4 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -299,6 +299,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
 
 	int fd = open(MAG_DEVICE_PATH, 0);
+
+	
+	
 	struct mag_scale mscale_null = {
 		0.0f,
 		1.0f,
@@ -396,6 +399,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	 * to shift the center to zero
 	 *
 	 * offset = max - ((max - min) / 2.0f)
+	 *          max - max/2 + min/2
+	 *          max/2 + min/2
 	 *
 	 * which reduces to
 	 *
@@ -422,6 +427,18 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
 		}
 
+		if (param_set(param_find("SENS_MAG_XSCALE"), &(mag_offset[0]))) {
+			fprintf(stderr, "[commander] Setting X mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_YSCALE"), &(mag_offset[1]))) {
+			fprintf(stderr, "[commander] Setting Y mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mag_offset[2]))) {
+			fprintf(stderr, "[commander] Setting Z mag offset failed!\n");
+		}
+
 		fd = open(MAG_DEVICE_PATH, 0);
 		struct mag_scale mscale = {
 			mag_offset[0],
diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h
index 88381aaaa..7ba9dd695 100644
--- a/apps/drivers/drv_mag.h
+++ b/apps/drivers/drv_mag.h
@@ -105,4 +105,7 @@ ORB_DECLARE(sensor_mag);
 /** perform self-calibration, update scale factors to canonical units */
 #define MAGIOCCALIBRATE		_MAGIOC(5)
 
+/** excite strap */
+#define MAGIOCEXSTRAP		_MAGIOC(6)
+
 #endif /* _DRV_MAG_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 893119407..fc80c8d17 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -187,6 +187,18 @@ private:
 	int			calibrate(unsigned enable);
 
 	/**
+	 * Perform the on-sensor scale calibration routine.
+	 *
+	 * @note The sensor will continue to provide measurements, these
+	 *	 will however reflect the uncalibrated sensor state until
+	 *	 the calibration routine has been completed.
+	 *
+	 * @param enable set to 1 to enable self-test positive strap, -1 to enable
+	 *        negative strap, 0 to set to normal mode
+	 */
+	int			set_excitement(unsigned enable);
+
+	/**
 	 * Set the sensor range.
 	 *
 	 * Sets the internal range to handle at least the argument in Gauss.
@@ -593,6 +605,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCCALIBRATE:
 		return calibrate(arg);
 
+	case MAGIOCEXSTRAP:
+		return set_excitement(arg);
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -800,14 +815,149 @@ out:
 
 int HMC5883::calibrate(unsigned enable)
 {
+	struct mag_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = (int)enable;
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+	if (sz != sizeof(report))
+		err(1, "immediate read failed");
+
+	warnx("starting mag scale calibration");
+	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	warnx("time:        %lld", report.timestamp);
+	warnx("sampling 500 samples for scaling offset");
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
+		errx(1, "failed to set queue depth");
+
+	/* start the sensor polling at 50 Hz */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50))
+		errx(1, "failed to set 2Hz poll rate");
+
+	/* Set to 2.5 Gauss */
+	if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
+		warnx("failed to set 2.5 Ga range");
+	}
+
+	if (OK != ioctl(fd, MAGIOCPOSEX, 1)) {
+		warnx("failed to enable sensor calibration mode");
+	}
+
+	struct mag_scale mscale_previous = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to get scale / offsets for mag");
+	}
+
+	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 null scale / offsets for mag");
+	}
+
+	float avg_excited[3];
+	unsigned i;
+
+	/* read the sensor 10x and report each value */
+	for (i = 0; i < 500; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1)
+			errx(1, "timed out waiting for sensor data");
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			err(1, "periodic read failed");
+		} else {
+			avg_excited[0] += report.x;
+			avg_excited[1] += report.y;
+			avg_excited[2] += report.z;
+		}
+
+		// warnx("periodic read %u", i);
+		// warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+		// warnx("time:        %lld", report.timestamp);
+	}
+
+	avg_excited[0] /= i;
+	avg_excited[1] /= i;
+	avg_excited[2] /= i;
+
+	warnx("periodic excited reads %u", i);
+	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+
+	/* Set to 1.1 Gauss and end calibration */
+	ret = ioctl(fd, MAGIOCNONEX, 0);
+	ret = ioctl(fd, MAGIOCSRANGE, 1);
+
+	float scaling[3];
+
+	/* calculate axis scaling */
+	scaling[0] = 1.16f / avg_excited[0];
+	/* second axis inverted */
+	scaling[1] = 1.16f / -avg_excited[1];
+	scaling[2] = 1.08f / avg_excited[2];
+
+	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
+	
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+	}
+
+	if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) {
+		warnx("failed to disable sensor calibration mode");
+	}
+
+	/* set scaling in device */
+	mscale_previous.x_scale = scaling[0];
+	mscale_previous.y_scale = scaling[1];
+	mscale_previous.z_scale = scaling[2];
+
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to set new scale / offsets for mag");
+	}
+}
+
+int HMC5883::set_excitement(unsigned enable)
+{
 	int ret;
 	/* arm the excitement strap */
 	uint8_t conf_reg;
 	ret = read_reg(ADDR_CONF_A, conf_reg);
 	if (OK != ret)
 		perf_count(_comms_errors);
-	if (enable) {
+	if (enable < 0) {
 		conf_reg |= 0x01;
+	} else if (enable > 0) {
+		conf_reg |= 0x02;
 	} else {
 		conf_reg &= ~0x03;
 	}
@@ -1020,7 +1170,6 @@ test()
  */
 int calibrate()
 {
-
 	struct mag_report report;
 	ssize_t sz;
 	int ret;
@@ -1029,216 +1178,11 @@ int calibrate()
 	if (fd < 0)
 		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
 
-	/* do a simple demand read */
-	sz = read(fd, &report, sizeof(report));
-	if (sz != sizeof(report))
-		err(1, "immediate read failed");
-
-	warnx("single read");
-	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	warnx("time:        %lld", report.timestamp);
-
-	/* set the queue depth to 10 */
-	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
-		errx(1, "failed to set queue depth");
-
-	/* start the sensor polling at 10 Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10))
-		errx(1, "failed to set 2Hz poll rate");
-
-	/* Set to 2.5 Gauss */
-	if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
-		warnx("failed to set 2.5 Ga range");
-	}
-
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) {
+	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
 		warnx("failed to enable sensor calibration mode");
 	}
 
-	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 null scale / offsets for mag");
-	}
-
-	float avg_excited[3];
-	unsigned i;
-
-	/* read the sensor 10x and report each value */
-	for (i = 0; i < 10; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1)
-			errx(1, "timed out waiting for sensor data");
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			err(1, "periodic read failed");
-		} else {
-			avg_excited[0] += report.x;
-			avg_excited[1] += report.y;
-			avg_excited[2] += report.z;
-		}
-
-		warnx("periodic read %u", i);
-		warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-		warnx("time:        %lld", report.timestamp);
-	}
-
-	// warnx("starting calibration");
-
-	// struct mag_report report;
-	// ssize_t sz;
-	// int ret;
-
-	// int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-	// if (fd < 0)
-	// 	err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
-
-	// /* do a simple demand read */
-	// sz = read(fd, &report, sizeof(report));
-	// if (sz != sizeof(report))
-	// 	err(1, "immediate read failed");
-
-	// warnx("single read");
-	// warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	// warnx("time:        %lld", report.timestamp);
-
-	// /* get scaling, set to zero */
-	// struct mag_scale mscale_previous;
-
-	// if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-	// 	warn("WARNING: failed to get scale / offsets for mag");
-	// }
-
-	// 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 null scale / offsets for mag");
-	// }
-
-	// warnx("sensor ready");
-
-	// float avg_excited[3] = {0.0f, 0.0f, 0.0f};
-
-	// if (OK != ioctl(fd, MAGIOCCALIBRATE, 1)) {
-	// 	warnx("failed to enable sensor calibration mode");
-	// }
-
-	// /* Set to 2.5 Gauss */
-	// if (OK != ioctl(fd, MAGIOCSRANGE, 2)) {
-	// 	warnx("failed to set 2.5 Ga range");
-	// }
-
-	// /* set the queue depth to 10 */
-	// if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
-	// 	warnx("failed to set queue depth");
-	// 	return 1;
-	// } else {
-	// 	warnx("set queue depth");
-	// }
-
-	// /* start the sensor polling at 100Hz */
-	// if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 100)) {
-	// 	warnx("failed to set 100 Hz poll rate");
-	// 	return 1;
-	// } else {
-	// 	warnx("set 100 Hz poll rate");
-	// }
-
-	// int i;
-	// for (i = 0; i < 10; i++) {
-	// 	struct pollfd fds;
-
-	// 	(void) ioctl(fd, MAGIOCCALIBRATE, 1);
-
-	// 	/* wait for data to be ready */
-	// 	fds.fd = fd;
-	// 	fds.events = POLLIN;
-	// 	ret = poll(&fds, 1, 2000);
-
-	// 	if (ret != 1) {
-	// 		warnx("timed out waiting for sensor data");
-	// 		return 1;
-	// 	}
-
-	// 	/* now go get it */
-	// 	sz = read(fd, &report, sizeof(report));
-
-	// 	if (sz != sizeof(report)) {
-	// 		warn("periodic read failed");
-	// 		return 1;
-	// 	} else {
-	// 		avg_excited[0] += report.x;
-	// 		avg_excited[1] += report.y;
-	// 		avg_excited[2] += report.z;
-	// 	}
-	// 	warnx("excited read %u", i);
-	// 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-	// 	warnx("time:        %lld", report.timestamp);
-
-	// }
-
-	avg_excited[0] /= i;
-	avg_excited[1] /= i;
-	avg_excited[2] /= i;
-
-	warnx("periodic excited reads %u", i);
-	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
-
-	/* Set to 1.1 Gauss and end calibration */
-	ret = ioctl(fd, MAGIOCCALIBRATE, 0);
-	ret = ioctl(fd, MAGIOCSRANGE, 1);
-
-	float scaling[3];
-
-	/* calculate axis scaling */
-	scaling[0] = 1.16f / avg_excited[0];
-	/* second axis inverted */
-	scaling[1] = 1.16f / -avg_excited[1];
-	scaling[2] = 1.08f / avg_excited[2];
-
-	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-	
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-	}
-
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, 0)) {
-		warnx("failed to disable sensor calibration mode");
-	}
-
-	/* set scaling in device */
-	// mscale_previous.x_scale = scaling[0];
-	// mscale_previous.y_scale = scaling[1];
-	// mscale_previous.z_scale = scaling[2];
-
-	// if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-	// 	warn("WARNING: failed to set new scale / offsets for mag");
-	// }
+	close(fd);
 
 	errx(0, "PASS");
 }
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 52316993e..5e8c5746c 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -52,6 +52,10 @@ 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(SENS_MAG_XSCALE, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 0.0f);
+PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 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);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index b84b58406..9a90d7fc5 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +34,7 @@
 
 /**
  * @file sensors.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Sensor readout process.
  */
@@ -178,6 +179,7 @@ private:
 
 		float gyro_offset[3];
 		float mag_offset[3];
+		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
 
@@ -209,6 +211,7 @@ private:
 		param_t accel_offset[3];
 		param_t accel_scale[3];
 		param_t mag_offset[3];
+		param_t mag_scale[3];
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -414,6 +417,10 @@ Sensors::Sensors() :
 	_parameter_handles.mag_offset[1] = param_find("SENS_MAG_YOFF");
 	_parameter_handles.mag_offset[2] = param_find("SENS_MAG_ZOFF");
 
+	_parameter_handles.mag_scale[0] = param_find("SENS_MAG_XSCALE");
+	_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
+	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
+
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
 	/* fetch initial parameter values */
@@ -537,6 +544,10 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
 	param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1]));
 	param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
+	/* mag scaling */
+	param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_scale[0]));
+	param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_scale[1]));
+	param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_scale[2]));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -798,11 +809,11 @@ Sensors::parameter_update_poll(bool forced)
 		fd = open(MAG_DEVICE_PATH, 0);
 		struct mag_scale mscale = {
 			_parameters.mag_offset[0],
-			1.0f,
+			_parameters.mag_scale[0],
 			_parameters.mag_offset[1],
-			1.0f,
+			_parameters.mag_scale[1],
 			_parameters.mag_offset[2],
-			1.0f,
+			_parameters.mag_scale[2],
 		};
 		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
 			warn("WARNING: failed to set scale / offsets for mag");