diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index fc6265ba4b..4b1d18e76b 100644 --- a/src/drivers/drv_sensor.h +++ b/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 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 6eb5bbe492..07bf93cddc 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/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) diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig index 39ed3db84a..018b89502b 100644 --- a/src/drivers/magnetometer/Kconfig +++ b/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" diff --git a/src/drivers/magnetometer/st/Kconfig b/src/drivers/magnetometer/st/Kconfig new file mode 100644 index 0000000000..f5acb5b616 --- /dev/null +++ b/src/drivers/magnetometer/st/Kconfig @@ -0,0 +1,3 @@ +menu "ST" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt b/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt new file mode 100644 index 0000000000..041f142ca1 --- /dev/null +++ b/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 + ) diff --git a/src/drivers/magnetometer/st/iis2mdc/Kconfig b/src/drivers/magnetometer/st/iis2mdc/Kconfig new file mode 100644 index 0000000000..fdd140c17e --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC + bool "iis2mdc" + default n + ---help--- + Enable support for iis2mdc diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp new file mode 100644 index 0000000000..23ce5ac539 --- /dev/null +++ b/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); +} diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h new file mode 100644 index 0000000000..40da40469d --- /dev/null +++ b/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 +#include + +// 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 +{ +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; +}; diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp new file mode 100644 index 0000000000..c3cd02813c --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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" +#include + +class IIS2MDC_I2C : public device::I2C +{ +public: + IIS2MDC_I2C(const I2CSPIDriverConfig &config); + virtual ~IIS2MDC_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count) override; + virtual int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); +}; + +IIS2MDC_I2C::IIS2MDC_I2C(const I2CSPIDriverConfig &config) : + I2C(config) +{ +} + +int IIS2MDC_I2C::probe() +{ + uint8_t data = 0; + + if (read(IIS2MDC_ADDR_WHO_AM_I, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + if (data != IIS2MDC_WHO_AM_I) { + DEVICE_DEBUG("IIS2MDC bad ID: %02x", data); + return -EIO; + } + + _retries = 1; + + return OK; +} + +int IIS2MDC_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int IIS2MDC_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); +} + +device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config) +{ + return new IIS2MDC_I2C(config); +} diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp new file mode 100644 index 0000000000..09ff5eceff --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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" +#include + +I2CSPIDriverBase *IIS2MDC::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + device::Device *interface = IIS2MDC_I2C_interface(config); + + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%lx)", config.bus, config.spi_devid); + return nullptr; + } + + IIS2MDC *dev = new IIS2MDC(interface, config); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +void IIS2MDC::print_usage() +{ + PRINT_MODULE_USAGE_NAME("iis2mdc", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int iis2mdc_main(int argc, char *argv[]) +{ + using ThisDriver = IIS2MDC; + int ch; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x1E; + cli.default_i2c_frequency = 400000; + + 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_IIS2MDC); + + 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; +}