add ST LSM303AGR driver (mag only)

src/drivers/drv_sensor.h

@@ -106,6 +106,8 @@
 #define DRV_BARO_DEVTYPE_LPS22HB	0x58
 #define DRV_ACC_DEVTYPE_ADIS16477	0x59
 #define DRV_GYR_DEVTYPE_ADIS16477	0x60
+#define DRV_ACC_DEVTYPE_LSM303AGR	0x61
+#define DRV_MAG_DEVTYPE_LSM303AGR	0x62
 
 /*
  * ioctl() definitions

src/drivers/magnetometer/CMakeLists.txt

@@ -35,3 +35,4 @@ add_subdirectory(bmm150)
 add_subdirectory(hmc5883)
 add_subdirectory(ist8310)
 add_subdirectory(lis3mdl)
+add_subdirectory(lsm303agr)

src/drivers/magnetometer/lsm303agr/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__magnetometer__lsm303agr
+	MAIN lsm303agr
+	SRCS
+		LSM303AGR.cpp
+	)

src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp

@@ -0,0 +1,657 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LSM303agr.cpp
+ * Driver for the ST LSM303AGR MEMS accelerometer / magnetometer connected via SPI.
+ * NOTE: currently only the mag is implemented
+ */
+
+#include "LSM303AGR.hpp"
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <ecl/geo/geo.h>
+
+/* SPI protocol address bits */
+#define DIR_READ				(1<<7)
+#define DIR_WRITE				(0<<7)
+#define ADDR_INCREMENT			(1<<6)
+
+#define LSM303AGR_DEVICE_PATH_MAG	"/dev/lsm303agr_mag"
+
+/* Max measurement rate is 100Hz */
+#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+
+static constexpr uint8_t LSM303AGR_WHO_AM_I_M = 0x40;
+
+/*
+  we set the timer interrupt to run a bit faster than the desired
+  sample rate and then throw away duplicates using the data ready bit.
+  This time reduction is enough to cope with worst case timing jitter
+  due to other timers
+ */
+#define LSM303AGR_TIMER_REDUCTION				200
+
+extern "C" { __EXPORT int lsm303agr_main(int argc, char *argv[]); }
+
+LSM303AGR::LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation) :
+	SPI("LSM303AGR", path, bus, device, SPIDEV_MODE3, 8 * 1000 * 1000),
+	_mag_sample_perf(perf_alloc(PC_ELAPSED, "LSM303AGR_mag_read")),
+	_bad_registers(perf_alloc(PC_COUNT, "LSM303AGR_bad_reg")),
+	_bad_values(perf_alloc(PC_COUNT, "LSM303AGR_bad_val")),
+	_rotation(rotation)
+{
+	/* Prime _mag with parents devid. */
+	_device_id.devid = _device_id.devid;
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303AGR;
+
+	_mag_scale.x_offset = 0.0f;
+	_mag_scale.x_scale = 1.0f;
+	_mag_scale.y_offset = 0.0f;
+	_mag_scale.y_scale = 1.0f;
+	_mag_scale.z_offset = 0.0f;
+	_mag_scale.z_scale = 1.0f;
+}
+
+LSM303AGR::~LSM303AGR()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	if (_class_instance != -1) {
+		unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance);
+	}
+
+	/* delete the perf counter */
+	perf_free(_mag_sample_perf);
+	perf_free(_bad_registers);
+	perf_free(_bad_values);
+}
+
+int
+LSM303AGR::init()
+{
+	int ret = PX4_OK;
+
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK) {
+		PX4_ERR("SPI init failed");
+		return PX4_ERROR;
+	}
+
+	_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
+
+	reset();
+
+	self_test();
+
+	reset();
+
+	/* fill report structures */
+	measure();
+
+	return ret;
+}
+
+int
+LSM303AGR::reset()
+{
+	// Single mode
+	// Output data rate configuration: 100Hz
+	write_reg(CFG_REG_A_M, CFG_REG_A_M_MD0 | CFG_REG_A_M_ODR1 | CFG_REG_A_M_ODR0);
+
+	// Enable low pass filter
+	write_reg(CFG_REG_B_M, CFG_REG_B_M_OFF_CANC | CFG_REG_B_M_OFF_LPF);
+
+	// Disable I2C
+	write_reg(CFG_REG_C_M, CFG_REG_C_M_I2C_DIS);
+
+	return PX4_OK;
+}
+
+bool
+LSM303AGR::self_test()
+{
+	// Magnetometer self-test procedure (LSM303AGR DocID027765 Rev 5 page 25/68)
+
+	uint8_t status_m = 0;
+
+	write_reg(CFG_REG_A_M, 0x0C);
+	write_reg(CFG_REG_B_M, 0x02);
+	write_reg(CFG_REG_C_M, 0x10);
+
+	// sleep 20ms
+	usleep(20000);
+
+	uint16_t OUTX_NOST = 0;
+	uint16_t OUTY_NOST = 0;
+	uint16_t OUTZ_NOST = 0;
+
+	// Check Zyxda 50 times and discard
+	// average x, y, z
+	for (int i = 0; i < 50; i++) {
+
+		status_m = read_reg(STATUS_REG_M);
+
+		OUTX_NOST += read_reg(OUTX_L_REG_M) + (read_reg(OUTX_H_REG_M) << 8);
+		OUTY_NOST += read_reg(OUTY_L_REG_M) + (read_reg(OUTY_H_REG_M) << 8);
+		OUTZ_NOST += read_reg(OUTZ_L_REG_M) + (read_reg(OUTZ_H_REG_M) << 8);
+	}
+
+	// enable self-test
+	write_reg(CFG_REG_C_M, 0x12);
+
+	// wait for 60ms
+	usleep(60000);
+
+	// Check Zyxda
+	status_m = read_reg(STATUS_REG_M);
+
+	// Read mag x, y, z to clear Zyxda bit
+	read_reg(OUTX_L_REG_M);
+	read_reg(OUTX_H_REG_M);
+	read_reg(OUTY_L_REG_M);
+	read_reg(OUTY_H_REG_M);
+	read_reg(OUTZ_L_REG_M);
+	read_reg(OUTZ_H_REG_M);
+
+	uint16_t OUTX_ST = 0;
+	uint16_t OUTY_ST = 0;
+	uint16_t OUTZ_ST = 0;
+
+	// Read the output registers after checking the Zyxda bit 50 times
+	// average x, y, z
+	for (int i = 0; i < 50; i++) {
+
+		status_m = read_reg(STATUS_REG_M);
+
+		OUTX_NOST += read_reg(OUTX_L_REG_M) + (read_reg(OUTX_H_REG_M) << 8);
+		OUTY_NOST += read_reg(OUTY_L_REG_M) + (read_reg(OUTY_H_REG_M) << 8);
+		OUTZ_NOST += read_reg(OUTZ_L_REG_M) + (read_reg(OUTZ_H_REG_M) << 8);
+	}
+
+	const uint16_t abs_x = abs(OUTX_ST - OUTX_NOST);
+	const uint16_t abs_y = abs(OUTY_ST - OUTY_NOST);
+	const uint16_t abs_z = abs(OUTZ_ST - OUTZ_NOST);
+
+	// TODO: proper ranges?
+	const bool x_valid = (abs_x > 0 && abs_x < UINT16_MAX);
+	const bool y_valid = (abs_y > 0 && abs_y < UINT16_MAX);
+	const bool z_valid = (abs_z > 0 && abs_z < UINT16_MAX);
+
+	if (!x_valid || !y_valid || !z_valid) {
+
+		PX4_ERR("self-test failed");
+
+		PX4_INFO("STATUS_M: %X", status_m);
+		PX4_INFO("ABS(OUTX_ST - OUTX_NOST) = %d", abs_x);
+		PX4_INFO("ABS(OUTY_ST - OUTY_NOST) = %d", abs_y);
+		PX4_INFO("ABS(OUTZ_ST - OUTZ_NOST) = %d", abs_z);
+	}
+
+	// disable self test
+	write_reg(CFG_REG_C_M, 0x10);
+
+	// Idle mode
+	write_reg(CFG_REG_A_M, 0x03);
+
+	return true;
+}
+
+int
+LSM303AGR::probe()
+{
+	/* verify that the device is attached and functioning */
+	bool success = (read_reg(WHO_AM_I_M) == LSM303AGR_WHO_AM_I_M);
+
+	if (success) {
+		return OK;
+	}
+
+	return -EIO;
+}
+
+int
+LSM303AGR::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(CONVERSION_INTERVAL)) {
+						return -EINVAL;
+					}
+
+					/* update interval for next measurement */
+					_measure_ticks = ticks;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		if (_measure_ticks == 0) {
+			return SENSOR_POLLRATE_MANUAL;
+		}
+
+		return 1000000 / TICK2USEC(_measure_ticks);
+
+	case SENSORIOCRESET:
+		return reset();
+
+	case MAGIOCSSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return ioctl(filp, SENSORIOCSPOLLRATE, arg);
+
+	case MAGIOCGSAMPLERATE:
+		/* same as pollrate because device is in single measurement mode*/
+		return 1000000 / TICK2USEC(_measure_ticks);
+
+	case MAGIOCSSCALE:
+		/* set new scale factors */
+		memcpy(&_mag_scale, (struct mag_calibration_s *)arg, sizeof(_mag_scale));
+
+		return 0;
+
+	case MAGIOCGSCALE:
+		/* copy out scale factors */
+		memcpy((struct mag_calibration_s *)arg, &_mag_scale, sizeof(_mag_scale));
+		return 0;
+
+	case MAGIOCGEXTERNAL:
+		DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
+		return external();
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+LSM303AGR::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+	cmd[1] = 0;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+LSM303AGR::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+LSM303AGR::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&LSM303AGR::cycle_trampoline, this, 1);
+}
+
+void
+LSM303AGR::stop()
+{
+	if (_measure_ticks > 0) {
+		/* ensure no new items are queued while we cancel this one */
+		_measure_ticks = 0;
+		work_cancel(HPWORK, &_work);
+	}
+}
+
+void
+LSM303AGR::cycle_trampoline(void *arg)
+{
+	LSM303AGR *dev = (LSM303AGR *)arg;
+
+	dev->cycle();
+}
+
+void
+LSM303AGR::cycle()
+{
+	if (_measure_ticks == 0) {
+		return;
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			DEVICE_DEBUG("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&LSM303AGR::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	measure();
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	if (_measure_ticks > 0) {
+		/* schedule a fresh cycle call when the measurement is done */
+		work_queue(HPWORK,  &_work, (worker_t)&LSM303AGR::cycle_trampoline, this, USEC2TICK(CONVERSION_INTERVAL));
+	}
+}
+
+void
+LSM303AGR::measure()
+{
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	write_reg(CFG_REG_A_M, CFG_REG_A_M_MD0 | CFG_REG_A_M_ODR1 | CFG_REG_A_M_ODR0);
+}
+
+int
+LSM303AGR::collect()
+{
+	const uint8_t status = read_reg(STATUS_REG_M);
+
+	// only publish new data
+	if (status & STATUS_REG_M_Zyxda) {
+		/* start the performance counter */
+		perf_begin(_mag_sample_perf);
+
+		mag_report mag_report = {};
+		mag_report.timestamp = hrt_absolute_time();
+
+		// no auto increment for mag
+		mag_report.x_raw = read_reg(OUTX_L_REG_M) + (read_reg(OUTX_H_REG_M) << 8);
+		mag_report.y_raw = read_reg(OUTY_L_REG_M) + (read_reg(OUTY_H_REG_M) << 8);
+		mag_report.z_raw = read_reg(OUTZ_L_REG_M) + (read_reg(OUTZ_H_REG_M) << 8);
+
+		float xraw_f = mag_report.x_raw;
+		float yraw_f = mag_report.y_raw;
+		float zraw_f = mag_report.z_raw;
+
+		/* apply user specified rotation */
+		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+
+		mag_report.x = ((xraw_f) - _mag_scale.x_offset) * _mag_scale.x_scale;
+		mag_report.y = ((yraw_f) - _mag_scale.y_offset) * _mag_scale.y_scale;
+		mag_report.z = ((zraw_f) - _mag_scale.z_offset) * _mag_scale.z_scale;
+		mag_report.scaling = 1.0f;
+		mag_report.range_ga = (float)_mag_range_ga;
+		mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
+
+		/* remember the temperature. The datasheet isn't clear, but it
+		 * seems to be a signed offset from 25 degrees C in units of 0.125C
+		 */
+		//mag_report.temperature = 25 + (raw_mag_report.temperature * 0.125f);;
+		mag_report.device_id = _device_id.devid;
+		mag_report.is_external = external();
+
+		if (!(_pub_blocked)) {
+
+			if (_mag_topic != nullptr) {
+				/* publish it */
+				orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+
+			} else {
+				_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, &_mag_orb_class_instance,
+								 (mag_report.is_external) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
+
+				if (_mag_topic == nullptr) {
+					DEVICE_DEBUG("ADVERT FAIL");
+				}
+			}
+		}
+
+
+		/* stop the perf counter */
+		perf_end(_mag_sample_perf);
+	}
+
+	return OK;
+}
+
+void
+LSM303AGR::print_info()
+{
+	perf_print_counter(_mag_sample_perf);
+	perf_print_counter(_bad_registers);
+	perf_print_counter(_bad_values);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace lsm303agr
+{
+
+LSM303AGR	*g_dev;
+
+void	start(enum Rotation rotation);
+void	info();
+void	usage();
+
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver is
+ * up and running or failed to detect the sensor.
+ */
+void
+start(enum Rotation rotation)
+{
+	int fd_mag = -1;
+
+	if (g_dev != nullptr) {
+		errx(0, "already started");
+	}
+
+	/* create the driver */
+#if defined(PX4_SPIDEV_LSM303A_M) && defined(PX4_SPIDEV_LSM303A_X)
+	g_dev = new LSM303AGR(PX4_SPI_BUS_SENSOR5, LSM303AGR_DEVICE_PATH_MAG, PX4_SPIDEV_LSM303A_M, rotation);
+#else
+	errx(0, "External SPI not available");
+#endif
+
+	if (g_dev == nullptr) {
+		PX4_ERR("alloc failed");
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	fd_mag = px4_open(LSM303AGR_DEVICE_PATH_MAG, O_RDONLY);
+
+	/* don't fail if open cannot be opened */
+	if (0 <= fd_mag) {
+		if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+			goto fail;
+		}
+	}
+
+	px4_close(fd_mag);
+
+	exit(0);
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running\n");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+void
+usage()
+{
+	PX4_INFO("missing command: try 'start', 'info', 'reset'");
+	PX4_INFO("options:");
+	PX4_INFO("    -X    (external bus)");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace
+
+int
+lsm303agr_main(int argc, char *argv[])
+{
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+
+		default:
+			lsm303agr::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
+	/*
+	 * Start/load the driver.
+
+	 */
+	if (!strcmp(verb, "start")) {
+		lsm303agr::start(rotation);
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(verb, "info")) {
+		lsm303agr::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'info'");
+}

src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp

@@ -0,0 +1,209 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_
+#define DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_
+
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/drv_mag.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
+#include <perf/perf_counter.h>
+#include <px4_workqueue.h>
+
+// Register mapping
+static constexpr uint8_t WHO_AM_I_M = 0x4F;
+
+static constexpr uint8_t CFG_REG_A_M = 0x60;
+
+struct CFG_REG_A_M_BITS {
+	uint8_t
+	: 1, // unused
+	: 1, // unused
+	SOFT_RST	: 1,
+	LP			: 1,
+	ODR1		: 1,
+	ODR0		: 1,
+	MD1			: 1,
+	MD0			: 1;
+};
+
+static constexpr uint8_t CFG_REG_A_M_SOFT_RST	= (1 << 5);
+static constexpr uint8_t CFG_REG_A_M_ODR1		= (1 << 3);
+static constexpr uint8_t CFG_REG_A_M_ODR0		= (1 << 2);
+static constexpr uint8_t CFG_REG_A_M_MD1		= (1 << 1);
+static constexpr uint8_t CFG_REG_A_M_MD0		= (1 << 0);
+
+static constexpr uint8_t CFG_REG_B_M = 0x61;
+static constexpr uint8_t CFG_REG_B_M_OFF_CANC	= (1 << 1);
+static constexpr uint8_t CFG_REG_B_M_OFF_LPF	= (1 << 0);
+
+static constexpr uint8_t CFG_REG_C_M = 0x62;
+static constexpr uint8_t CFG_REG_C_M_I2C_DIS	= (1 << 5);
+static constexpr uint8_t CFG_REG_C_M_BDU		= (1 << 4);
+static constexpr uint8_t CFG_REG_C_M_Self_test	= (1 << 1);
+
+static constexpr uint8_t STATUS_REG_M = 0x67;
+static constexpr uint8_t STATUS_REG_M_Zyxda		= (1 << 3);
+
+// Magnetometer output registers
+static constexpr uint8_t OUTX_L_REG_M = 0x68;
+static constexpr uint8_t OUTX_H_REG_M = 0x69;
+static constexpr uint8_t OUTY_L_REG_M = 0x6A;
+static constexpr uint8_t OUTY_H_REG_M = 0x6B;
+static constexpr uint8_t OUTZ_L_REG_M = 0x6C;
+static constexpr uint8_t OUTZ_H_REG_M = 0x6D;
+
+class LSM303AGR : public device::SPI
+{
+public:
+	LSM303AGR(int bus, const char *path, uint32_t device, enum Rotation rotation);
+	virtual ~LSM303AGR();
+
+	virtual int		init();
+
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+
+	work_s			_work{};
+	bool			_collect_phase{false};
+
+	unsigned		_measure_ticks{0};
+
+	unsigned		_call_mag_interval{0};
+
+	mag_calibration_s	_mag_scale{};
+	unsigned		_mag_range_ga{0};
+
+	int			_class_instance{-1};
+
+	unsigned		_mag_samplerate{100};
+
+	perf_counter_t		_mag_sample_perf;
+	perf_counter_t		_bad_registers;
+	perf_counter_t		_bad_values;
+
+	enum Rotation		_rotation;
+
+	orb_advert_t		_mag_topic{nullptr};
+	int					_mag_orb_class_instance{-1};
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Reset chip.
+	 *
+	 * Resets the chip and measurements ranges, but not scale and offset.
+	 */
+	int				reset();
+
+
+	bool			self_test();
+
+	/**
+	 * Issue a measurement command.
+	 *
+	 * @return		OK if the measurement command was successful.
+	 */
+	void			measure();
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int			collect();
+
+	/**
+	 * Perform a poll cycle; collect from the previous measurement
+	 * and start a new one.
+	 *
+	 * This is the heart of the measurement state machine.  This function
+	 * alternately starts a measurement, or collects the data from the
+	 * previous measurement.
+	 *
+	 * When the interval between measurements is greater than the minimum
+	 * measurement interval, a gap is inserted between collection
+	 * and measurement to provide the most recent measurement possible
+	 * at the next interval.
+	 */
+	void			cycle();
+
+	/**
+	 * Static trampoline from the workq context; because we don't have a
+	 * generic workq wrapper yet.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		cycle_trampoline(void *arg);
+
+	/**
+	 * Read a register from the LSM303AGR
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the LSM303AGR
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/* this class cannot be copied */
+	LSM303AGR(const LSM303AGR &);
+	LSM303AGR operator=(const LSM303AGR &);
+};
+
+#endif /* DRIVERS_IMU_LSM303AGR_LSM303AGR_HPP_ */