Complete refactor of the lis3mdl driver. This has been bench tested, flight tested, and verified functional.

src/drivers/magnetometer/lis3mdl/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_module(
 	SRCS
 		lis3mdl_i2c.cpp
 		lis3mdl_spi.cpp
+		lis3mdl_main.cpp
 		lis3mdl.cpp
 	DEPENDS
 	)

src/drivers/magnetometer/lis3mdl/lis3mdl.h

@@ -39,10 +39,270 @@
 
 #pragma once
 
-#define ADDR_WHO_AM_I			0x0f
-#define ID_WHO_AM_I				0x3d
+#include <float.h>
+#include <getopt.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mag.h>
+
+#include <lib/conversion/rotation.h>
+
+#include <perf/perf_counter.h>
+#include <px4_defines.h>
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+/**
+ * LIS3MDL internal constants and data structures.
+ */
+
+/* Max measurement rate is 80Hz */
+#define LIS3MDL_CONVERSION_INTERVAL     (1000000 / 80)  /* 12,500 microseconds */
+
+#define NUM_BUS_OPTIONS                 (sizeof(bus_options)/sizeof(bus_options[0]))
+
+#define ADDR_WHO_AM_I                   0x0f
+#define ID_WHO_AM_I                     0x3d
+
+#define ADDR_CTRL_REG1                  0x20
+#define ADDR_CTRL_REG2                  0x21
+#define ADDR_CTRL_REG3                  0x22
+#define ADDR_CTRL_REG4                  0x23
+#define ADDR_CTRL_REG5                  0x24
+
+#define ADDR_STATUS_REG                 0x27
+#define ADDR_OUT_X_L                    0x28
+#define ADDR_OUT_X_H                    0x29
+#define ADDR_OUT_Y_L                    0x2a
+#define ADDR_OUT_Y_H                    0x2b
+#define ADDR_OUT_Z_L                    0x2c
+#define ADDR_OUT_Z_H                    0x2d
+#define ADDR_OUT_T_L                    0x2e
+#define ADDR_OUT_T_H                    0x2f
+
+#define MODE_REG_CONTINOUS_MODE         (0 << 0)
+#define MODE_REG_SINGLE_MODE            (1 << 0) /* default */
+
+#define CNTL_REG1_DEFAULT 0xFC
+#define CNTL_REG2_DEFAULT 0x00
+#define CNTL_REG3_DEFAULT 0x00
+#define CNTL_REG4_DEFAULT 0x0C
+#define CNTL_REG5_DEFAULT 0x00
 
 /* interface factories */
 extern device::Device *LIS3MDL_SPI_interface(int bus);
 extern device::Device *LIS3MDL_I2C_interface(int bus);
 typedef device::Device *(*LIS3MDL_constructor)(int);
+
+enum LIS3MDL_BUS {
+	LIS3MDL_BUS_ALL = 0,
+	LIS3MDL_BUS_I2C_INTERNAL,
+	LIS3MDL_BUS_I2C_EXTERNAL,
+	LIS3MDL_BUS_SPI
+};
+
+enum OPERATING_MODE {
+	CONTINUOUS = 0,
+	SINGLE
+};
+
+
+class LIS3MDL : public device::CDev
+{
+public:
+	LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation);
+
+	virtual ~LIS3MDL();
+
+	virtual int init();
+
+	virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void print_info();
+
+	/**
+	 * Configures the device with default register values.
+	 */
+	int set_default_register_values();
+
+	/**
+	 * Stop the automatic measurement state machine.
+	 */
+	void stop();
+
+protected:
+	Device *_interface;
+
+private:
+	work_s _work;
+
+	ringbuffer::RingBuffer *_reports;
+
+	struct mag_calibration_s _scale;
+
+	struct mag_report _last_report {};      /**< used for info() */
+
+	orb_advert_t _mag_topic;
+
+	perf_counter_t _comms_errors;
+	perf_counter_t _conf_errors;
+	perf_counter_t _range_errors;
+	perf_counter_t _sample_perf;
+
+	/* status reporting */
+	bool _calibrated;                       /**< the calibration is valid */
+	bool _continuous_mode_set;
+
+	enum OPERATING_MODE _mode;
+	enum Rotation _rotation;
+
+	unsigned int _measure_ticks;
+
+	int _class_instance;
+	int _orb_class_instance;
+
+	float _range_ga;
+	float _range_scale;
+
+	uint8_t _check_state_cnt;
+	uint8_t _cntl_reg1;
+	uint8_t _cntl_reg2;
+	uint8_t _cntl_reg3;
+	uint8_t _cntl_reg4;
+	uint8_t _cntl_reg5;
+	uint8_t _range_bits;
+	uint8_t _temperature_counter;
+	uint8_t _temperature_error_count;
+
+
+	/**
+	 * @brief Performs 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 strap, 0 to disable
+	 */
+	int calibrate(struct file *filp, unsigned enable);
+
+	/**
+	 * Collect the result of the most recent measurement.
+	 */
+	int collect();
+
+	/**
+	 * Check the current calibration and update device status
+	 *
+	 * @return 0 if calibration is ok, 1 else
+	 */
+	int check_calibration();
+
+	/**
+	* Check the current scale calibration
+	*
+	* @return 0 if scale calibration is ok, 1 else
+	*/
+	int check_scale();
+
+	/**
+	* Check the current offset calibration
+	*
+	* @return 0 if offset calibration is ok, 1 else
+	*/
+	int check_offset();
+
+	/**
+	 * @brief Performs 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();
+
+	/**
+	 * @brief 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);
+
+	/**
+	 * Issue a measurement command.
+	 *
+	 * @return              OK if the measurement command was successful.
+	 */
+	int measure();
+
+	/**
+	 * @brief Resets the device
+	 */
+	int reset();
+
+	/**
+	 * @brief Initialises the automatic measurement state machine and start it.
+	 *
+	 * @note This function is called at open and error time.  It might make sense
+	 *       to make it more aggressive about resetting the bus in case of errors.
+	 */
+	void start();
+
+	/**
+	 * @brief Performs 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);
+
+	/**
+	 * @brief Sets the sensor internal range to handle at least the argument in Gauss.
+	 *
+	 * @param range The sensor range value to be set.
+	 */
+	int set_range(unsigned range);
+
+	/**
+	 * @brief Reads a register.
+	 *
+	 * @param reg           The register to read.
+	 * @param val           The value read.
+	 * @return              OK on read success.
+	 */
+	int read_reg(uint8_t reg, uint8_t &val);
+
+	/**
+	 * @brief  Writes a register.
+	 *
+	 * @param reg           The register to write.
+	 * @param val           The value to write.
+	 * @return              OK on write success.
+	 */
+	int write_reg(uint8_t reg, uint8_t val);
+
+	/* this class has pointer data members, do not allow copying it */
+	LIS3MDL(const LIS3MDL &);
+
+	LIS3MDL operator=(const LIS3MDL &);
+}; // class LIS3MDL

src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp

@@ -37,16 +37,15 @@
  * I2C interface for LIS3MDL
  */
 
-/* XXX trim includes */
 #include <px4_config.h>
 
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <string.h>
 #include <assert.h>
 #include <debug.h>
 #include <errno.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
 #include <unistd.h>
 
 #include <arch/board/board.h>
@@ -55,14 +54,12 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
-#include "lis3mdl.h"
 #include "board_config.h"
+#include "lis3mdl.h"
 
 #if defined(PX4_I2C_BUS_ONBOARD) || defined(PX4_I2C_BUS_EXPANSION)
 
-#define LIS3MDLL_ADDRESS		0x1e
-
-device::Device *LIS3MDL_I2C_interface(int bus);
+#define LIS3MDLL_ADDRESS        0x1e
 
 class LIS3MDL_I2C : public device::I2C
 {
@@ -70,17 +67,19 @@ public:
 	LIS3MDL_I2C(int bus);
 	virtual ~LIS3MDL_I2C();
 
-	virtual int	init();
-	virtual int	read(unsigned address, void *data, unsigned count);
-	virtual int	write(unsigned address, void *data, unsigned count);
-
-	virtual int	ioctl(unsigned operation, unsigned &arg);
+	virtual int     init();
+	virtual int     ioctl(unsigned operation, unsigned &arg);
+	virtual int     read(unsigned address, void *data, unsigned count);
+	virtual int     write(unsigned address, void *data, unsigned count);
 
 protected:
-	virtual int	probe();
+	virtual int     probe();
 
 };
 
+device::Device *
+LIS3MDL_I2C_interface(int bus);
+
 device::Device *
 LIS3MDL_I2C_interface(int bus)
 {
@@ -107,23 +106,17 @@ LIS3MDL_I2C::init()
 int
 LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg)
 {
-	int ret;
-
 	switch (operation) {
 
 	case MAGIOCGEXTERNAL:
-		external();
-
-	/* FALLTHROUGH */
+		return external();
 
 	case DEVIOCGDEVICEID:
 		return CDev::ioctl(nullptr, operation, arg);
 
 	default:
-		ret = -EINVAL;
+		return  -EINVAL;
 	}
-
-	return ret;
 }
 
 int
@@ -148,6 +141,13 @@ LIS3MDL_I2C::probe()
 	return OK;
 }
 
+int
+LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
 int
 LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
 {
@@ -163,11 +163,4 @@ LIS3MDL_I2C::write(unsigned address, void *data, unsigned count)
 	return transfer(&buf[0], count + 1, nullptr, 0);
 }
 
-int
-LIS3MDL_I2C::read(unsigned address, void *data, unsigned count)
-{
-	uint8_t cmd = address;
-	return transfer(&cmd, 1, (uint8_t *)data, count);
-}
-
 #endif /* PX4_I2C_OBDEV_LIS3MDL */

src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp

@@ -0,0 +1,405 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 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 lis3mdl_main.cpp
+ *
+ * Driver for the LIS3MDL magnetometer connected via I2C or SPI.
+ */
+
+#include "lis3mdl_main.h"
+
+/**
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int lis3mdl_main(int argc, char *argv[]);
+
+int
+lis3mdl::calibrate(LIS3MDL_BUS bus_id)
+{
+	int ret;
+	struct lis3mdl_bus_option &bus = find_bus(bus_id);
+	const char *path = bus.devpath;
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_WARN("%s open failed (try 'lis3mdl start' if the driver is not running", path);
+		return 1;
+	}
+
+	if ((ret = ioctl(fd, MAGIOCCALIBRATE, fd)) != OK) {
+		PX4_WARN("failed to enable sensor calibration mode");
+	}
+
+	close(fd);
+
+	return ret;
+}
+
+int
+lis3mdl::info(LIS3MDL_BUS bus_id)
+{
+	struct lis3mdl_bus_option &bus = find_bus(bus_id);
+
+	PX4_WARN("running on bus: %u (%s)\n", (unsigned)bus.bus_id, bus.devpath);
+	bus.dev->print_info();
+	return 1;
+}
+
+bool
+lis3mdl::init(LIS3MDL_BUS bus_id)
+{
+	struct lis3mdl_bus_option &bus = find_bus(bus_id);
+	const char *path = bus.devpath;
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0) {
+		return false;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		close(fd);
+		errx(1, "Failed to setup poll rate");
+		return false;
+
+	} else {
+		PX4_INFO("Poll rate set to max (80hz)");
+	}
+
+	/* Set to 4 Gauss */
+	if (ioctl(fd, MAGIOCSRANGE, 4) != OK) {
+		PX4_WARN("FAILED: MAGIOCSRANGE 4 Gauss");
+
+	} else {
+		PX4_INFO("Set mag range to 4 Gauss");
+	}
+
+	close(fd);
+
+	return true;
+}
+
+bool
+lis3mdl::start_bus(struct lis3mdl_bus_option &bus, Rotation rotation)
+{
+	if (bus.dev != nullptr) {
+		errx(1, "bus option already started");
+		return false;
+	}
+
+	device::Device *interface = bus.interface_constructor(bus.busnum);
+
+	if (interface->init() != OK) {
+		delete interface;
+		warnx("no device on bus %u", (unsigned)bus.bus_id);
+		return false;
+	}
+
+	bus.dev = new LIS3MDL(interface, bus.devpath, rotation);
+
+	if (bus.dev != nullptr &&
+	    bus.dev->init() != OK) {
+		delete bus.dev;
+		bus.dev = NULL;
+		return false;
+	}
+
+	return true;
+}
+
+int
+lis3mdl::start(LIS3MDL_BUS bus_id, Rotation rotation)
+{
+	bool started = false;
+
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (bus_id == LIS3MDL_BUS_ALL && bus_options[i].dev != NULL) {
+			// this device is already started
+			continue;
+		}
+
+		if (bus_id != LIS3MDL_BUS_ALL && bus_options[i].bus_id != bus_id) {
+			// not the one that is asked for
+			continue;
+		}
+
+		started |= start_bus(bus_options[i], rotation);
+		//init(bus_id);
+	}
+
+	return started;
+}
+
+int
+lis3mdl::stop()
+{
+	bool stopped = false;
+
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if (bus_options[i].dev != nullptr) {
+			bus_options[i].dev->stop();
+			delete bus_options[i].dev;
+			bus_options[i].dev = nullptr;
+			stopped = true;
+		}
+	}
+
+	return !stopped;
+}
+
+bool
+lis3mdl::test(LIS3MDL_BUS bus_id)
+{
+	struct lis3mdl_bus_option &bus = find_bus(bus_id);
+	struct mag_report report;
+	ssize_t sz;
+	int ret;
+	const char *path = bus.devpath;
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_WARN("%s open failed (try 'lis3mdl start')", path);
+		return 1;
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		PX4_WARN("immediate read failed");
+		return 1;
+	}
+
+	print_message(report);
+
+	/* check if mag is onboard or external */
+	if (ioctl(fd, MAGIOCGEXTERNAL, 0) < 0) {
+		PX4_WARN("failed to get if mag is onboard or external");
+		return 1;
+	}
+
+	/* set the queue depth to 5 */
+	if (ioctl(fd, SENSORIOCSQUEUEDEPTH, 10) != OK) {
+		PX4_WARN("failed to set queue depth");
+		return 1;
+	}
+
+	/* start the sensor polling at 2Hz */
+	if (ioctl(fd, SENSORIOCSPOLLRATE, 2) != OK) {
+		PX4_WARN("failed to set 2Hz poll rate");
+		return 1;
+	}
+
+	struct pollfd fds;
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			PX4_WARN("timed out waiting for sensor data");
+			return 1;
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			PX4_WARN("periodic read failed");
+			return 1;
+		}
+
+		print_message(report);
+	}
+
+	PX4_INFO("PASS");
+	return 1;
+}
+
+bool
+lis3mdl::reset(LIS3MDL_BUS bus_id)
+{
+	struct lis3mdl_bus_option &bus = find_bus(bus_id);
+	const char *path = bus.devpath;
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_WARN("open failed ");
+		return 1;
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		PX4_WARN("driver reset failed");
+		return 1;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		PX4_WARN("driver poll restart failed");
+		return 1;
+	}
+
+	return 0;
+}
+
+void
+lis3mdl::usage()
+{
+	PX4_WARN("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
+	PX4_WARN("options:");
+	PX4_WARN("    -R rotation");
+	PX4_WARN("    -C calibrate on start");
+	PX4_WARN("    -X only external bus");
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS)
+	PX4_WARN("    -I only internal bus");
+#endif
+}
+
+int
+lis3mdl_main(int argc, char *argv[])
+{
+	int index = 0;
+	bool calibrate = false;
+
+	enum LIS3MDL_BUS bus_id = LIS3MDL_BUS_ALL;
+	enum Rotation rotation = ROTATION_NONE;
+
+	while ((index = getopt(argc, argv, "XISR:CT")) != EOF) {
+		switch (index) {
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS)
+
+		case 'I':
+			bus_id = LIS3MDL_BUS_I2C_INTERNAL;
+			break;
+#endif
+
+		case 'X':
+			bus_id = LIS3MDL_BUS_I2C_EXTERNAL;
+			break;
+
+		case 'S':
+			bus_id = LIS3MDL_BUS_SPI;
+			break;
+
+		case 'C':
+			calibrate = true;
+			break;
+
+		default:
+			lis3mdl::usage();
+			return 0;
+		}
+	}
+
+	const char *arg = argv[optind];
+
+	// Start/load the driver
+	if (!strcmp(arg, "start")) {
+
+		if (lis3mdl::start(bus_id, rotation)) {
+			if (calibrate) {
+				if (OK != lis3mdl::calibrate(bus_id)) {
+					PX4_WARN("calibration failed");
+					return 1;
+				}
+			}
+
+			lis3mdl::init(bus_id);
+
+			return 0;
+
+		} else {
+			return 1;
+		}
+	}
+
+	// Stop the driver
+	if (!strcmp(arg, "stop")) {
+		return lis3mdl::stop();
+	}
+
+	// Test the driver/device
+	if (!strcmp(arg, "test")) {
+		return lis3mdl::test(bus_id);
+	}
+
+	// Reset the driver
+	if (!strcmp(arg, "reset")) {
+		return lis3mdl::reset(bus_id);
+	}
+
+	// Print driver information
+	if (!strcmp(arg, "info") ||
+	    !strcmp(arg, "status")) {
+		return lis3mdl::info(bus_id);
+	}
+
+	// Autocalibrate the scaling
+	if (!strcmp(arg, "calibrate")) {
+		if (lis3mdl::calibrate(bus_id) == 0) {
+			PX4_INFO("calibration successful");
+			return 0;
+
+		} else {
+			PX4_INFO("calibration failed");
+			return 1;
+		}
+
+	}
+
+	PX4_INFO("unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'");
+	return 1;
+}
+
+struct
+lis3mdl::lis3mdl_bus_option &lis3mdl::find_bus(LIS3MDL_BUS bus_id)
+{
+	for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
+		if ((bus_id == LIS3MDL_BUS_ALL ||
+		     bus_id == bus_options[i].bus_id) && bus_options[i].dev != NULL) {
+			return bus_options[i];
+		}
+	}
+
+	errx(1, "bus %u not started", (unsigned)bus_id);
+}

src/drivers/magnetometer/lis3mdl/lis3mdl_main.h

@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 lis3mdl_main.h
+ */
+
+#pragma once
+
+#include "lis3mdl.h"
+
+namespace lis3mdl
+{
+/**
+ * @struct List of supported bus configurations
+ */
+struct lis3mdl_bus_option {
+	LIS3MDL_BUS bus_id;
+	const char *devpath;
+	LIS3MDL_constructor interface_constructor;
+	uint8_t busnum;
+	LIS3MDL *dev;
+} bus_options[] = {
+#ifdef PX4_I2C_BUS_EXPANSION
+	{ LIS3MDL_BUS_I2C_EXTERNAL, "/dev/lis3mdl_ext", &LIS3MDL_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
+#endif /* PX4_I2C_BUS_EXPANSION */
+#ifdef PX4_I2C_BUS_ONBOARD
+	{ LIS3MDL_BUS_I2C_INTERNAL, "/dev/lis3mdl_int", &LIS3MDL_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
+#endif /* PX4_I2C_BUS_ONBOARD */
+#ifdef PX4_SPIDEV_LIS
+	{ LIS3MDL_BUS_SPI, "/dev/lis3mdl_spi", &LIS3MDL_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
+#endif /* PX4_SPIDEV_LIS */
+};
+
+/**
+ * @brief Finds a bus structure for a bus_id
+ */
+lis3mdl_bus_option &find_bus(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Calibrate and self test. Self test feature cannot be used to calculate scale.
+ *
+ * SELF TEST OPERATION
+ * Note: To check the LIS3MDL for proper operation, a self test feature is incorporated :
+ *       sensor offset straps are excited to create a nominal field strength
+ *       (bias field) to be measured. To implement self test, the least significant bits
+ *       (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias).
+ *       A few measurements are taken and stored with and without the additional magnetic
+ *       field. According to ST datasheet, those values must stay between thresholds in order
+ *       to pass the self test.
+ */
+int calibrate(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Prints info about the driver.
+ */
+int info(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Initializes the driver -- sets defaults and starts a cycle
+ */
+bool init(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Resets the driver.
+ */
+bool reset(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Starts the driver for a specific bus option
+ */
+bool start_bus(struct lis3mdl_bus_option &bus, Rotation rotation);
+
+/**
+ * @brief Starts the driver. This function call only returns once the driver
+ *        is either successfully up and running or failed to start.
+ */
+int start(LIS3MDL_BUS bus_id, Rotation rotation);
+
+/**
+ * @brief Stop the driver.
+ */
+int stop();
+
+/**
+ * @brief Perform some basic functional tests on the driver;
+ * 	  make sure we can collect data from the sensor in polled
+ * 	  and automatic modes.
+ */
+bool test(LIS3MDL_BUS bus_id);
+
+/**
+ * @brief Prints info about the driver argument usage.
+ */
+void usage();
+
+} // namespace lis3mdl

src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp

@@ -37,16 +37,15 @@
  * SPI interface for LIS3MDL
  */
 
-/* XXX trim includes */
 #include <px4_config.h>
 
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <string.h>
 #include <assert.h>
 #include <debug.h>
 #include <errno.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <sys/types.h>
 #include <unistd.h>
 
 #include <arch/board/board.h>
@@ -55,17 +54,15 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_device.h>
 
-#include "lis3mdl.h"
 #include "board_config.h"
+#include "lis3mdl.h"
 
 #ifdef PX4_SPIDEV_LIS
 
 /* SPI protocol address bits */
-#define DIR_READ			(1<<7)
-#define DIR_WRITE			(0<<7)
-#define ADDR_INCREMENT			(1<<6)
-
-device::Device *LIS3MDL_SPI_interface(int bus);
+#define DIR_READ        (1<<7)
+#define DIR_WRITE       (0<<7)
+#define ADDR_INCREMENT  (1<<6)
 
 class LIS3MDL_SPI : public device::SPI
 {
@@ -73,12 +70,15 @@ public:
 	LIS3MDL_SPI(int bus, uint32_t device);
 	virtual ~LIS3MDL_SPI();
 
-	virtual int	init();
-	virtual int	read(unsigned address, void *data, unsigned count);
-	virtual int	write(unsigned address, void *data, unsigned count);
-	virtual int	ioctl(unsigned operation, unsigned &arg);
+	virtual int     init();
+	virtual int     ioctl(unsigned operation, unsigned &arg);
+	virtual int     read(unsigned address, void *data, unsigned count);
+	virtual int     write(unsigned address, void *data, unsigned count);
 };
 
+device::Device *
+LIS3MDL_SPI_interface(int bus);
+
 device::Device *
 LIS3MDL_SPI_interface(int bus)
 {
@@ -148,21 +148,6 @@ LIS3MDL_SPI::ioctl(unsigned operation, unsigned &arg)
 	return ret;
 }
 
-int
-LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
-
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
-
-	buf[0] = address | DIR_WRITE;
-	memcpy(&buf[1], data, count);
-
-	return transfer(&buf[0], &buf[0], count + 1);
-}
-
 int
 LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
 {
@@ -179,4 +164,19 @@ LIS3MDL_SPI::read(unsigned address, void *data, unsigned count)
 	return ret;
 }
 
+int
+LIS3MDL_SPI::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_WRITE;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], &buf[0], count + 1);
+}
+
 #endif /* PX4_SPIDEV_LIS */