Merge pull request #11 from cvg/fmuv2_bringup_ext_mag_rotation

Support for external mags with different rotation

4 files changed, 49 insertions, 10 deletions

diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 9aab995a1..e95034e8e 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag);
 /** perform self test and report status */
 #define MAGIOCSELFTEST		_MAGIOC(7)
 
+/** determine if mag is external or onboard */
+#define MAGIOCGEXTERNAL		_MAGIOC(8)
+
 #endif /* _DRV_MAG_H */
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 039b496f4..692f890bd 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -167,6 +167,8 @@ private:
 	bool			_sensor_ok;		/**< sensor was found and reports ok */
 	bool			_calibrated;		/**< the calibration is valid */
 
+	int			_bus;			/**< the bus the device is connected to */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) :
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
 	_sensor_ok(false),
-	_calibrated(false)
+	_calibrated(false),
+	_bus(bus)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MAGIOCSELFTEST:
 		return check_calibration();
 
+	case MAGIOCGEXTERNAL:
+		if (_bus == PX4_I2C_BUS_EXPANSION)
+			return 1;
+		else
+			return 0;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -851,8 +860,9 @@ HMC5883::collect()
 		_reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
 	} else {
 #endif
-		/* XXX axis assignment of external sensor is yet unknown */
-		_reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+		/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
+		 * therefore switch and invert x and y */
+		_reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale;
 		/* flip axes and negate value for y */
 		_reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale;
 		/* z remains z */
@@ -1293,6 +1303,11 @@ test()
 	warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
 	warnx("time:        %lld", report.timestamp);
 
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
 	/* set the queue depth to 10 */
 	if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10))
 		errx(1, "failed to set queue depth");
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 117583faf..3e6ce64b8 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
 //		return self_test();
 		return -EINVAL;
 
+	case MAGIOCGEXTERNAL:
+		/* no external mag board yet */
+		return 0;
+
 	default:
 		/* give it to the superclass */
 		return SPI::ioctl(filp, cmd, arg);
@@ -1422,7 +1426,7 @@ test()
 	int fd_accel = -1;
 	struct accel_report accel_report;
 	ssize_t sz;
-	int filter_bandwidth;
+	int ret;
 
 	/* get the driver */
 	fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -1445,10 +1449,10 @@ test()
 	warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
 
 	warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
-	if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
+	if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
 		warnx("accel antialias filter bandwidth: fail");
 	else
-		warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth);
+		warnx("accel antialias filter bandwidth: %d Hz", ret);
 
 	int fd_mag = -1;
 	struct mag_report m_report;
@@ -1459,6 +1463,11 @@ test()
 	if (fd_mag < 0)
 		err(1, "%s open failed", MAG_DEVICE_PATH);
 
+	/* check if mag is onboard or external */
+	if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
+		errx(1, "failed to get if mag is onboard or external");
+	warnx("device active: %s", ret ? "external" : "onboard");
+
 	/* do a simple demand read */
 	sz = read(fd_mag, &m_report, sizeof(m_report));
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c47f6bb7d..198da9f0a 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -284,6 +284,7 @@ private:
 
 	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -537,7 +538,8 @@ Sensors::Sensors() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
 	_board_rotation(3,3),
-	_external_mag_rotation(3,3)
+	_external_mag_rotation(3,3),
+	_mag_is_external(false)
 {
 
 	/* basic r/c parameters */
@@ -948,6 +950,14 @@ Sensors::mag_init()
 	/* set the driver to poll at 150Hz */
 	ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
+	int ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	if (ret < 0)
+		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+	else if (ret == 1)
+		_mag_is_external = true;
+	else
+		_mag_is_external = false;
+
 	close(fd);
 }
 
@@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		// XXX TODO add support for external mag orientation
-
 		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
-		vect = _board_rotation*vect;
+
+		if (_mag_is_external)
+			vect = _external_mag_rotation*vect;
+		else
+			vect = _board_rotation*vect;
 
 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);