drivers: add lis2mdl mag driver

src/drivers/drv_sensor.h

@@ -63,6 +63,7 @@
 #define DRV_MAG_DEVTYPE_AK09916  0x09
 #define DRV_IMU_DEVTYPE_ICM20948 0x0A
 #define DRV_MAG_DEVTYPE_IST8308  0x0B
+#define DRV_MAG_DEVTYPE_LIS2MDL  0x0C
 
 #define DRV_IMU_DEVTYPE_LSM303D  0x11
 #define DRV_ACC_DEVTYPE_BMA180   0x12
@@ -160,6 +161,7 @@
 #define DRV_MAG_DEVTYPE_UAVCAN	0x88
 #define DRV_DIST_DEVTYPE_UAVCAN	0x89
 
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/magnetometer/lis2mdl/CMakeLists.txt

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2020 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__lis2mdl
+	MAIN lis2mdl
+	SRCS
+		lis2mdl_i2c.cpp
+		lis2mdl_spi.cpp
+		lis2mdl_main.cpp
+		lis2mdl.cpp
+	)

src/drivers/magnetometer/lis2mdl/lis2mdl.cpp

@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 lis2mdl.cpp
+ *
+ * Driver for the LIS2MDL magnetometer connected via I2C or SPI.
+ *
+ * Based on the LIS2MDL driver.
+ */
+
+#include <px4_platform_common/time.h>
+#include "lis2mdl.h"
+
+LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) :
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
+	_px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
+	_interface(interface),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
+	_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
+	_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
+	_measure_interval(0)
+{
+	_px4_mag.set_external(_interface->external());
+	_px4_mag.set_scale(0.0015f); /* 49.152f / (2^15) */
+}
+
+LIS2MDL::~LIS2MDL()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_range_errors);
+	perf_free(_conf_errors);
+}
+
+int
+LIS2MDL::measure()
+{
+	struct {
+		uint8_t status;
+		uint8_t x[2];
+		uint8_t y[2];
+		uint8_t z[2];
+	}       lis_report;
+
+	struct {
+		int16_t x;
+		int16_t y;
+		int16_t z;
+		int16_t t;
+	} report;
+
+	uint8_t buf_rx[2] = {};
+
+	_px4_mag.set_error_count(perf_event_count(_comms_errors));
+
+	perf_begin(_sample_perf);
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	int ret = _interface->read(ADDR_STATUS_REG, (uint8_t *)&lis_report, sizeof(lis_report));
+
+	/**
+	 * Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ.
+	 * The temperature registers must be addressed directly.
+	 */
+	ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));
+
+	if (ret != OK) {
+		perf_end(_sample_perf);
+		perf_count(_comms_errors);
+		PX4_WARN("Register read error.");
+		return ret;
+	}
+
+	perf_end(_sample_perf);
+
+	if ((lis_report.status & (1 << 3)) == 0) { // check data ready
+		return 0;
+	}
+
+	report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
+	report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
+	report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);
+
+	report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);
+
+	float temperature = report.t;
+	_px4_mag.set_temperature(25.0f + (temperature / 8.0f));
+
+	/* swap x and y axis */
+	_px4_mag.update(timestamp_sample, report.y, report.x, report.z);
+
+	return PX4_OK;
+}
+
+void
+LIS2MDL::RunImpl()
+{
+	/* _measure_interval == 0  is used as _task_should_exit */
+	if (_measure_interval == 0) {
+		return;
+	}
+
+	if (measure() != OK) {
+		PX4_DEBUG("measure error");
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	ScheduleDelayed(LIS2MDL_CONVERSION_INTERVAL);
+}
+
+int
+LIS2MDL::init()
+{
+
+	int ret = write_reg(ADDR_CFG_REG_A, CFG_REG_A_ODR | CFG_REG_A_MD | CFG_REG_A_TEMP_COMP_EN);
+	ret = write_reg(ADDR_CFG_REG_B, 0);
+	ret = write_reg(ADDR_CFG_REG_C, CFG_REG_C_BDU);
+
+	_measure_interval = LIS2MDL_CONVERSION_INTERVAL;
+	start();
+
+	return ret;
+}
+
+void
+LIS2MDL::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	PX4_INFO("poll interval:  %u", _measure_interval);
+	_px4_mag.print_status();
+}
+
+void
+LIS2MDL::start()
+{
+	/* schedule a cycle to start things */
+	ScheduleNow();
+}
+
+int
+LIS2MDL::read_reg(uint8_t reg, uint8_t &val)
+{
+	uint8_t buf = val;
+	int ret = _interface->read(reg, &buf, 1);
+	val = buf;
+	return ret;
+}
+
+int
+LIS2MDL::write_reg(uint8_t reg, uint8_t val)
+{
+	uint8_t buf = val;
+	return _interface->write(reg, &buf, 1);
+}

src/drivers/magnetometer/lis2mdl/lis2mdl.h

@@ -0,0 +1,154 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 lis2mdl.h
+ *
+ * Shared defines for the LIS2MDL driver.
+ */
+
+#pragma once
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mag.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+
+/**
+ * LIS2MDL internal constants and data structures.
+ */
+
+/* Max measurement rate is 50Hz */
+#define LIS2MDL_CONVERSION_INTERVAL     (1000000 / 50)
+
+#define ADDR_WHO_AM_I                   0x4f
+#define ID_WHO_AM_I                     0x40
+
+#define ADDR_CFG_REG_A                  0x60
+#define ADDR_CFG_REG_B                  0x61
+#define ADDR_CFG_REG_C                  0x62
+#define ADDR_INT_CTRL_REG               0x63
+
+#define ADDR_STATUS_REG                 0x67
+#define ADDR_OUT_X_L                    0x68
+#define ADDR_OUT_X_H                    0x69
+#define ADDR_OUT_Y_L                    0x6a
+#define ADDR_OUT_Y_H                    0x6b
+#define ADDR_OUT_Z_L                    0x6c
+#define ADDR_OUT_Z_H                    0x6d
+#define ADDR_OUT_T_L                    0x6e
+#define ADDR_OUT_T_H                    0x6f
+
+#define CFG_REG_A_TEMP_COMP_EN          (1 << 7)
+#define CFG_REG_A_ODR                   (2 << 2) /* 50Hz (100Hz creates spikes randomly) */
+#define CFG_REG_A_MD                    (0 << 0) /* continuous mode */
+
+#define CFG_REG_B_LPF                   (1 << 0) /* LPF */
+
+#define CFG_REG_C_BDU                   (1 << 4) /* avoids reading of incorrect data due to async reads */
+
+/* interface factories */
+extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
+
+
+class LIS2MDL : public I2CSPIDriver<LIS2MDL>
+{
+public:
+	LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
+	virtual ~LIS2MDL();
+
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
+
+	virtual int init();
+
+	void print_status() override;
+
+	/**
+	 * Configures the device with default register values.
+	 */
+	int set_default_register_values();
+
+	void RunImpl();
+
+private:
+	PX4Magnetometer _px4_mag;
+
+	device::Device *_interface;
+
+	perf_counter_t _comms_errors;
+	perf_counter_t _conf_errors;
+	perf_counter_t _range_errors;
+	perf_counter_t _sample_perf;
+
+	unsigned int _measure_interval;
+
+	/**
+	 * Issue a measurement command & publish data.
+	 *
+	 * @return              OK if the measurement command was successful.
+	 */
+	int measure();
+
+	/**
+	 * @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 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);
+
+};

src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 lis2mdl_i2c.cpp
+ *
+ * I2C interface for LIS2MDL
+ */
+
+#include <px4_platform_common/px4_config.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 <drivers/device/i2c.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
+
+#include "board_config.h"
+#include "lis2mdl.h"
+
+#define LIS2MDLL_ADDRESS        0x1e
+
+class LIS2MDL_I2C : public device::I2C
+{
+public:
+	LIS2MDL_I2C(int bus, int bus_frequency);
+	virtual ~LIS2MDL_I2C() = default;
+
+	virtual int     read(unsigned address, void *data, unsigned count);
+	virtual int     write(unsigned address, void *data, unsigned count);
+
+protected:
+	virtual int     probe();
+
+};
+
+device::Device *
+LIS2MDL_I2C_interface(int bus, int bus_frequency);
+
+device::Device *
+LIS2MDL_I2C_interface(int bus, int bus_frequency)
+{
+	return new LIS2MDL_I2C(bus, bus_frequency);
+}
+
+LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
+	I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
+{
+}
+
+int
+LIS2MDL_I2C::probe()
+{
+	uint8_t data = 0;
+
+	_retries = 10;
+
+	if (read(ADDR_WHO_AM_I, &data, 1)) {
+		DEVICE_DEBUG("read_reg fail");
+		return -EIO;
+	}
+
+	_retries = 2;
+
+	if (data != ID_WHO_AM_I) {
+		DEVICE_DEBUG("LIS2MDL bad ID: %02x", data);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+LIS2MDL_I2C::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t cmd = address;
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
+int
+LIS2MDL_I2C::write(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address;
+	memcpy(&buf[1], data, count);
+
+	return transfer(&buf[0], count + 1, nullptr, 0);
+}

src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 lis2mdl_main.cpp
+ *
+ * Driver for the LIS2MDL magnetometer connected via I2C or SPI.
+ */
+
+#include "lis2mdl.h"
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+I2CSPIDriverBase *LIS2MDL::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				       int runtime_instance)
+{
+	device::Device *interface = nullptr;
+
+	if (iterator.busType() == BOARD_I2C_BUS) {
+		interface = LIS2MDL_I2C_interface(iterator.bus(), cli.bus_frequency);
+
+	} else if (iterator.busType() == BOARD_SPI_BUS) {
+		interface = LIS2MDL_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
+	}
+
+	if (interface == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (interface->init() != OK) {
+		delete interface;
+		PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
+		return nullptr;
+	}
+
+	LIS2MDL *dev = new LIS2MDL(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
+
+	if (dev == nullptr) {
+		delete interface;
+		return nullptr;
+	}
+
+	if (OK != dev->init()) {
+		delete dev;
+		return nullptr;
+	}
+
+	return dev;
+}
+
+void LIS2MDL::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("lis2mdl", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int lis2mdl_main(int argc, char *argv[])
+{
+	using ThisDriver = LIS2MDL;
+	int ch;
+	BusCLIArguments cli{true, true};
+	cli.default_i2c_frequency = 400000;
+	cli.default_spi_frequency = 11 * 1000 * 1000;
+
+	while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
+		switch (ch) {
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optarg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optarg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp

@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 lis2mdl_spi.cpp
+ *
+ * SPI interface for LIS2MDL
+ */
+
+#include <px4_platform_common/px4_config.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 <drivers/device/spi.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_device.h>
+
+#include "board_config.h"
+#include "lis2mdl.h"
+
+/* SPI protocol address bits */
+#define DIR_READ        (1<<7)
+#define DIR_WRITE       (0<<7)
+#define ADDR_INCREMENT  (1<<6)
+
+class LIS2MDL_SPI : public device::SPI
+{
+public:
+	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	virtual ~LIS2MDL_SPI() = default;
+
+	virtual int     init();
+	virtual int     read(unsigned address, void *data, unsigned count);
+	virtual int     write(unsigned address, void *data, unsigned count);
+};
+
+device::Device *
+LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+
+device::Device *
+LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+{
+	return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
+}
+
+LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
+{
+}
+
+int
+LIS2MDL_SPI::init()
+{
+	int ret;
+
+	ret = SPI::init();
+
+	if (ret != OK) {
+		DEVICE_DEBUG("SPI init failed");
+		return -EIO;
+	}
+
+	// read WHO_AM_I value
+	uint8_t data = 0;
+
+	if (read(ADDR_WHO_AM_I, &data, 1)) {
+		DEVICE_DEBUG("LIS2MDL read_reg fail");
+	}
+
+	if (data != ID_WHO_AM_I) {
+		DEVICE_DEBUG("LIS2MDL bad ID: %02x", data);
+		return -EIO;
+	}
+
+	return OK;
+}
+
+int
+LIS2MDL_SPI::read(unsigned address, void *data, unsigned count)
+{
+	uint8_t buf[32];
+
+	if (sizeof(buf) < (count + 1)) {
+		return -EIO;
+	}
+
+	buf[0] = address | DIR_READ | ADDR_INCREMENT;
+
+	int ret = transfer(&buf[0], &buf[0], count + 1);
+	memcpy(data, &buf[1], count);
+	return ret;
+}
+
+int
+LIS2MDL_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);
+}