From 0b71aac5bf05dbb879b3b886d07b9db2ba8e76d5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 12 Feb 2025 14:12:33 -0500 Subject: [PATCH] drivers/magnetometer/st/iis2mdc: refactor to monitor registers - if there's any configuration error the driver is now able to reset/reconfigure itself --- .../magnetometer/st/iis2mdc/CMakeLists.txt | 9 +- .../magnetometer/st/iis2mdc/IIS2MDC.cpp | 321 ++++++++++++++++++ .../magnetometer/st/iis2mdc/IIS2MDC.hpp | 114 +++++++ src/drivers/magnetometer/st/iis2mdc/Kconfig | 2 +- .../{iis2mdc.h => ST_IIS2MDC_registers.hpp} | 119 ++++--- .../magnetometer/st/iis2mdc/iis2mdc.cpp | 137 -------- .../magnetometer/st/iis2mdc/iis2mdc_i2c.cpp | 97 ------ .../magnetometer/st/iis2mdc/iis2mdc_main.cpp | 45 +-- 8 files changed, 516 insertions(+), 328 deletions(-) create mode 100644 src/drivers/magnetometer/st/iis2mdc/IIS2MDC.cpp create mode 100644 src/drivers/magnetometer/st/iis2mdc/IIS2MDC.hpp rename src/drivers/magnetometer/st/iis2mdc/{iis2mdc.h => ST_IIS2MDC_registers.hpp} (50%) delete mode 100644 src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp delete mode 100644 src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp diff --git a/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt b/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt index 041f142ca1..8b644c2a5b 100644 --- a/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt +++ b/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2024-2025 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 @@ -30,15 +30,16 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( MODULE drivers__magnetometer__st__iis2mdc MAIN iis2mdc COMPILE_FLAGS - # -DDEBUG_BUILD SRCS - iis2mdc_i2c.cpp + IIS2MDC.cpp + IIS2MDC.hpp iis2mdc_main.cpp - iis2mdc.cpp + ST_IIS2MDC_registers.hpp DEPENDS drivers_magnetometer px4_work_queue diff --git a/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.cpp b/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.cpp new file mode 100644 index 0000000000..f0f03d92cb --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.cpp @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * Copyright (c) 2024-2025 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.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +IIS2MDC::IIS2MDC(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) +{ +} + +IIS2MDC::~IIS2MDC() +{ + perf_free(_reboot_perf); + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int IIS2MDC::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + PX4_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reboot() ? 0 : -1; +} + +bool IIS2MDC::Reboot() +{ + _state = STATE::REBOOT; + ScheduleClear(); + ScheduleNow(); + return true; +} + +bool IIS2MDC::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void IIS2MDC::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reboot_perf); + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int IIS2MDC::probe() +{ + for (int retry = 0; retry < 3; retry++) { + + uint8_t id = RegisterRead(Register::WHO_AM_I); + + if (id == Device_ID) { + _retries = 2; + return PX4_OK; + + } else { + PX4_DEBUG("unexpected WHO_AM_I 0x%02x", id); + } + } + + return PX4_ERROR; +} + +void IIS2MDC::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::REBOOT: + // CFG_REG_A: Reboot + RegisterWrite(Register::CFG_REG_A, CFG_REG_A_BIT::REBOOT); + perf_count(_reboot_perf); + _state = STATE::RESET; + ScheduleDelayed(40_ms); // Wait > 20 ms + break; + + case STATE::RESET: + // CFG_REG_A: Software Reset + RegisterWrite(Register::CFG_REG_A, CFG_REG_A_BIT::SOFT_RST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(10_ms); + break; + + case STATE::WAIT_FOR_RESET: + + // SOFT_RST: This bit is automatically reset to zero after reset + if ((RegisterRead(Register::WHO_AM_I) == Device_ID) + && ((RegisterRead(Register::CFG_REG_A) & CFG_REG_A_BIT::SOFT_RST) == 0)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start read cycle + _state = STATE::READ; + ScheduleOnInterval(20_ms); // 50 Hz + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + struct TransferBuffer { + uint8_t STATUS_REG; + uint8_t OUTX_L_REG; + uint8_t OUTX_H_REG; + uint8_t OUTY_L_REG; + uint8_t OUTY_H_REG; + uint8_t OUTZ_L_REG; + uint8_t OUTZ_H_REG; + uint8_t TEMP_OUT_L_REG; + uint8_t TEMP_OUT_H_REG; + } buffer{}; + + bool success = false; + uint8_t cmd = static_cast(Register::STATUS_REG); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + + // new set of data is available (Zyxda = 1) + if (buffer.STATUS_REG & STATUS_REG_BIT::Zyxda) { + int16_t x = combine(buffer.OUTX_H_REG, buffer.OUTX_L_REG); + int16_t y = combine(buffer.OUTY_H_REG, buffer.OUTY_L_REG); + int16_t z = combine(buffer.OUTZ_H_REG, buffer.OUTZ_L_REG); + + // sensor's frame is +x forward, +y right, +z up + z = math::negate(z); + + int16_t t = combine(buffer.TEMP_OUT_H_REG, buffer.TEMP_OUT_L_REG); + + // temperature sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C + const float temperature = (t / 8.f) + 25.f; + _px4_mag.set_temperature(temperature); + + _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); + _px4_mag.update(now, x, y, z); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reboot/reset if things are failing consistently + if (_failure_count > 10) { + Reboot(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reboot/reset + perf_count(_bad_register_perf); + Reboot(); + return; + } + } + } + + break; + } +} + +bool IIS2MDC::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_mag.set_scale(0.0015f); // M_So = 1.5 mG / LSB + + return success; +} + +bool IIS2MDC::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t IIS2MDC::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void IIS2MDC::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void IIS2MDC::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.hpp b/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.hpp new file mode 100644 index 0000000000..8d5a36ece0 --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/IIS2MDC.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2024-2025 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 IIS2MDC.hpp + * + * Driver for the ST IIS2MDC connected via I2C. + * + */ + +#pragma once + +#include "ST_IIS2MDC_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace ST_IIS2MDC; + +class IIS2MDC : public device::I2C, public I2CSPIDriver +{ +public: + IIS2MDC(const I2CSPIDriverConfig &config); + ~IIS2MDC() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reboot(); + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reboot_perf{perf_alloc(PC_COUNT, MODULE_NAME": reboot")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + enum class STATE : uint8_t { + REBOOT, + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{3}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CFG_REG_A, CFG_REG_A_BIT::COMP_TEMP_EN | CFG_REG_A_BIT::ODR_50_HZ_SET, CFG_REG_A_BIT::LP | CFG_REG_A_BIT::ODR_50_HZ_CLEAR | CFG_REG_A_BIT::MD_CONTINUOUS_CLEAR }, + { Register::CFG_REG_B, CFG_REG_B_BIT::OFF_CANC, CFG_REG_B_BIT::LPF }, + { Register::CFG_REG_C, CFG_REG_C_BIT::BDU, CFG_REG_C_BIT::BLE | CFG_REG_C_BIT::I2C_DIS }, + }; +}; diff --git a/src/drivers/magnetometer/st/iis2mdc/Kconfig b/src/drivers/magnetometer/st/iis2mdc/Kconfig index fdd140c17e..fa5071376b 100644 --- a/src/drivers/magnetometer/st/iis2mdc/Kconfig +++ b/src/drivers/magnetometer/st/iis2mdc/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC bool "iis2mdc" default n ---help--- - Enable support for iis2mdc + Enable support for ST IIS2MDC diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h b/src/drivers/magnetometer/st/iis2mdc/ST_IIS2MDC_registers.hpp similarity index 50% rename from src/drivers/magnetometer/st/iis2mdc/iis2mdc.h rename to src/drivers/magnetometer/st/iis2mdc/ST_IIS2MDC_registers.hpp index 40da40469d..5f7c163a32 100644 --- a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h +++ b/src/drivers/magnetometer/st/iis2mdc/ST_IIS2MDC_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024-2025 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 @@ -31,65 +31,78 @@ * ****************************************************************************/ +/** + * @file ST_IIS2MDC_registers.hpp + * + * ST IIS2MDC registers. + * + */ + #pragma once -#include -#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 +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); -// 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 +namespace ST_IIS2MDC { -public: - IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config); - virtual ~IIS2MDC(); +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b001'1110; - 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 constexpr uint8_t Device_ID = 0b0100'0000; - static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); - static void print_usage(); +enum class Register : uint8_t { + WHO_AM_I = 0x4F, - int init(); - void print_status() override; + CFG_REG_A = 0x60, + CFG_REG_B = 0x61, + CFG_REG_C = 0x62, - 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; + STATUS_REG = 0x67, }; + +// CFG_REG_A +enum CFG_REG_A_BIT : uint8_t { + COMP_TEMP_EN = Bit7, // internal temperature sensor + REBOOT = Bit6, + SOFT_RST = Bit5, + LP = Bit4, // Low-Power mode + + // 3:2 ODR: 10 50 Hz + ODR_50_HZ_SET = Bit3, // ODR1: 1 + ODR_50_HZ_CLEAR = Bit2, // ODR0: 0 + + // 1:0 MD: 00 Continuous mode + MD_CONTINUOUS_CLEAR = Bit1 | Bit0, // MD1: 0, MD0: 0 Continuous mode +}; + +// CFG_REG_B +enum CFG_REG_B_BIT : uint8_t { + OFF_CANC = Bit1, // offset cancellation + LPF = Bit0, // digital low-pass filter +}; + +// CFG_REG_C +enum CFG_REG_C_BIT : uint8_t { + I2C_DIS = Bit5, + BDU = Bit4, + BLE = Bit3, // 1: Big Endian +}; + +// STATUS_REG +enum STATUS_REG_BIT : uint8_t { + Zyxor = Bit7, // X, Y, Z axis data overrun + + Zyxda = Bit3, // X, Y, Z new data available +}; + + +} // namespace ST_IIS2MDC diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp deleted file mode 100644 index 23ce5ac539..0000000000 --- a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * 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_i2c.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp deleted file mode 100644 index c3cd02813c..0000000000 --- a/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * 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 index 09ff5eceff..99b7441b7e 100644 --- a/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2024-2025 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 @@ -31,56 +31,29 @@ * ****************************************************************************/ -#include "iis2mdc.h" +#include "IIS2MDC.hpp" + +#include #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_PARAMS_I2C_ADDRESS(0x1E); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } extern "C" int iis2mdc_main(int argc, char *argv[]) { - using ThisDriver = IIS2MDC; int ch; + using ThisDriver = IIS2MDC; BusCLIArguments cli{true, false}; - cli.i2c_address = 0x1E; - cli.default_i2c_frequency = 400000; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) {