drivers/magnetometer: new ST IIS2MDC Magnetometer driver

src/drivers/drv_sensor.h

@@ -64,6 +64,7 @@
 #define DRV_MAG_DEVTYPE_IST8308  0x0B
 #define DRV_MAG_DEVTYPE_LIS2MDL  0x0C
 #define DRV_MAG_DEVTYPE_MMC5983MA  0x0D
+#define DRV_MAG_DEVTYPE_IIS2MDC  0x0E
 
 #define DRV_IMU_DEVTYPE_LSM303D  0x11
 

src/drivers/magnetometer/CMakeLists.txt

@@ -41,4 +41,5 @@ add_subdirectory(lis3mdl)
 add_subdirectory(lsm303agr)
 add_subdirectory(memsic)
 add_subdirectory(rm3100)
+add_subdirectory(st)
 add_subdirectory(vtrantech)

src/drivers/magnetometer/Kconfig

@@ -14,6 +14,7 @@ menu "Magnetometer"
         select DRIVERS_MAGNETOMETER_RM3100
         select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
         select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
+        select DRIVERS_MAGNETOMETER_ST_IIS2MDC
         ---help---
             Enable default set of magnetometer drivers
     rsource "*/Kconfig"

src/drivers/magnetometer/st/Kconfig

@@ -0,0 +1,3 @@
+menu "ST"
+    rsource "*/Kconfig"
+endmenu

src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2024 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__st__iis2mdc
+	MAIN iis2mdc
+	COMPILE_FLAGS
+		# -DDEBUG_BUILD
+	SRCS
+		iis2mdc_i2c.cpp
+		iis2mdc_main.cpp
+		iis2mdc.cpp
+	DEPENDS
+		drivers_magnetometer
+		px4_work_queue
+	)

src/drivers/magnetometer/st/iis2mdc/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC
+	bool "iis2mdc"
+	default n
+	---help---
+		Enable support for iis2mdc

src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#include "iis2mdc.h"
+
+using namespace time_literals;
+
+IIS2MDC::IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config) :
+	I2CSPIDriver(config),
+	_interface(interface),
+	_px4_mag(interface->get_device_id(), config.rotation),
+	_sample_count(perf_alloc(PC_COUNT, "iis2mdc_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "iis2mdc_comms_errors"))
+{}
+
+IIS2MDC::~IIS2MDC()
+{
+	perf_free(_sample_count);
+	perf_free(_comms_errors);
+	delete _interface;
+}
+
+int IIS2MDC::init()
+{
+	if (hrt_absolute_time() < 20_ms) {
+		px4_usleep(20_ms); // ~10ms power-on time
+	}
+
+	write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN);
+	write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC);
+	write_register(IIS2MDC_ADDR_CFG_REG_C, BDU);
+
+	_px4_mag.set_scale(100.f / 65535.f); // +/- 50 Gauss, 16bit
+
+	ScheduleDelayed(20_ms);
+
+	return PX4_OK;
+}
+
+void IIS2MDC::RunImpl()
+{
+	uint8_t status = read_register(IIS2MDC_ADDR_STATUS_REG);
+
+	if (status & IIS2MDC_STATUS_REG_READY) {
+		SensorData data = {};
+
+		if (read_register_block(&data) == PX4_OK) {
+			int16_t x = int16_t((data.xout1 << 8) | data.xout0);
+			int16_t y = int16_t((data.yout1 << 8) | data.yout0);
+			int16_t z = -int16_t((data.zout1 << 8) | data.zout0);
+			int16_t t = int16_t((data.tout1 << 8) | data.tout0);
+			// 16 bits twos complement with a sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C.
+			_px4_mag.set_temperature(float(t) / 8.f + 25.f);
+			_px4_mag.update(hrt_absolute_time(), x, y, z);
+			_px4_mag.set_error_count(perf_event_count(_comms_errors));
+			perf_count(_sample_count);
+
+		} else {
+			PX4_DEBUG("read failed");
+			perf_count(_comms_errors);
+		}
+
+	} else {
+		PX4_DEBUG("not ready: %u", status);
+		perf_count(_comms_errors);
+	}
+
+	ScheduleDelayed(10_ms);
+}
+
+uint8_t IIS2MDC::read_register_block(SensorData *data)
+{
+	uint8_t reg = IIS2MDC_ADDR_OUTX_L_REG;
+
+	if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) {
+		perf_count(_comms_errors);
+
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+uint8_t IIS2MDC::read_register(uint8_t reg)
+{
+	uint8_t value = 0;
+
+	if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) {
+		perf_count(_comms_errors);
+	}
+
+	return value;
+}
+
+void IIS2MDC::write_register(uint8_t reg, uint8_t value)
+{
+	if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) {
+		perf_count(_comms_errors);
+	}
+}
+
+void IIS2MDC::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_count);
+	perf_print_counter(_comms_errors);
+}

src/drivers/magnetometer/st/iis2mdc/iis2mdc.h

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+
+// IIS2MDC Registers
+#define IIS2MDC_ADDR_CFG_REG_A  0x60
+#define IIS2MDC_ADDR_CFG_REG_B  0x61
+#define IIS2MDC_ADDR_CFG_REG_C  0x62
+#define IIS2MDC_ADDR_STATUS_REG 0x67
+#define IIS2MDC_ADDR_OUTX_L_REG 0x68
+#define IIS2MDC_ADDR_WHO_AM_I   0x4F
+
+// IIS2MDC Definitions
+#define IIS2MDC_WHO_AM_I         0b01000000
+#define IIS2MDC_STATUS_REG_READY 0b00001111
+// CFG_REG_A
+#define COMP_TEMP_EN    (1 << 7)
+#define MD_CONTINUOUS   (0 << 0)
+#define ODR_100         ((1 << 3) | (1 << 2))
+// CFG_REG_B
+#define OFF_CANC        (1 << 1)
+// CFG_REG_C
+#define BDU             (1 << 4)
+
+extern device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config);
+
+class IIS2MDC : public I2CSPIDriver<IIS2MDC>
+{
+public:
+	IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config);
+	virtual ~IIS2MDC();
+
+	struct SensorData {
+		uint8_t xout0;
+		uint8_t xout1;
+		uint8_t yout0;
+		uint8_t yout1;
+		uint8_t zout0;
+		uint8_t zout1;
+		uint8_t tout0;
+		uint8_t tout1;
+	};
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	int init();
+	void print_status() override;
+
+	void RunImpl();
+
+private:
+	uint8_t read_register_block(SensorData *data);
+	uint8_t read_register(uint8_t reg);
+	void write_register(uint8_t reg, uint8_t value);
+
+	device::Device *_interface;
+	PX4Magnetometer _px4_mag;
+	perf_counter_t _sample_count;
+	perf_counter_t _comms_errors;
+};