From 6750137e355791bd3b4e421979710fe6406c4f7b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 17 Nov 2024 20:54:01 -0900 Subject: [PATCH] AP_Compass: add IIS2MDC driver --- Tools/scripts/build_options.py | 1 + Tools/scripts/decode_devid.py | 1 + libraries/AP_Compass/AP_Compass.cpp | 23 ++- libraries/AP_Compass/AP_Compass.h | 3 + libraries/AP_Compass/AP_Compass_Backend.h | 1 + libraries/AP_Compass/AP_Compass_IIS2MDC.cpp | 175 ++++++++++++++++++++ libraries/AP_Compass/AP_Compass_IIS2MDC.h | 68 ++++++++ libraries/AP_Compass/AP_Compass_config.h | 4 + 8 files changed, 275 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Compass/AP_Compass_IIS2MDC.cpp create mode 100644 libraries/AP_Compass/AP_Compass_IIS2MDC.h diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index f31edfaf87ff3..97838da3750ac 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -171,6 +171,7 @@ def config_option(self): Feature('Compass', 'HMC5843', 'AP_COMPASS_HMC5843_ENABLED', 'Enable HMC5843 compasses', 1, None), Feature('Compass', 'ICM20948', 'AP_COMPASS_ICM20948_ENABLED', 'Enable AK09916 on ICM20948 compasses', 1, "AK09916"), Feature('Compass', 'IST8308', 'AP_COMPASS_IST8308_ENABLED', 'Enable IST8308 compasses', 1, None), + Feature('Compass', 'IIS2MDC', 'AP_COMPASS_IIS2MDC_ENABLED', 'Enable IIS2MDC compasses', 1, None), Feature('Compass', 'IST8310', 'AP_COMPASS_IST8310_ENABLED', 'Enable IST8310 compasses', 1, None), Feature('Compass', 'LIS3MDL', 'AP_COMPASS_LIS3MDL_ENABLED', 'Enable LIS3MDL compasses', 1, None), Feature('Compass', 'LSM303D', 'AP_COMPASS_LSM303D_ENABLED', 'Enable LSM303D compasses', 1, None), diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index 8841d06d8769e..d618c2d59296d 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -67,6 +67,7 @@ def num(s): 0x15 : "DEVTYPE_AK09915", 0x16 : "DEVTYPE_QMC5883P", 0x17 : "DEVTYPE_BMM350", + 0x18 : "DEVTYPE_IIS2MDC", } imu_types = { diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 7738ac7f7447e..695e139772b3a 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -23,6 +23,7 @@ #include "AP_Compass_BMM150.h" #include "AP_Compass_BMM350.h" #include "AP_Compass_HMC5843.h" +#include "AP_Compass_IIS2MDC.h" #include "AP_Compass_IST8308.h" #include "AP_Compass_IST8310.h" #include "AP_Compass_LSM303D.h" @@ -529,7 +530,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Param: DISBLMSK // @DisplayName: Compass disable driver type mask // @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup - // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS,19:MMC5XX3,20:QMC5883P,21:BMM350 + // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS,19:MMC5XX3,20:QMC5883P,21:BMM350,22:IIS2MDC // @User: Advanced AP_GROUPINFO("DISBLMSK", 33, Compass, _driver_type_mask, 0), @@ -1171,6 +1172,26 @@ void Compass::_probe_external_i2c_compasses(void) #endif #endif // AP_COMPASS_QMC5883P_ENABLED +#if AP_COMPASS_IIS2MDC_ENABLED + //external i2c bus + FOREACH_I2C_EXTERNAL(i) { + ADD_BACKEND(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IIS2MDC_I2C_ADDR), + true, HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL)); + } + + // internal i2c bus +#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE) + if (all_external) { + // only probe IIS2MDC on internal if we are treating internals as externals + FOREACH_I2C_INTERNAL(i) { + ADD_BACKEND(DRIVER_IIS2MDC, AP_Compass_IIS2MDC::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IIS2MDC_I2C_ADDR), + all_external, + all_external?HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL:HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL)); + } + } +#endif +#endif // AP_COMPASS_QMC5883P_ENABLED + #ifndef HAL_BUILD_AP_PERIPH // AK09916 on ICM20948 #if AP_COMPASS_AK09916_ENABLED && AP_COMPASS_ICM20948_ENABLED diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 6498c1d31cd43..75e8e4698466c 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -499,6 +499,9 @@ friend class AP_Compass_Backend; #if AP_COMPASS_BMM350_ENABLED DRIVER_BMM350 =21, #endif +#if AP_COMPASS_IIS2MDC_ENABLED + DRIVER_IIS2MDC =22, +#endif }; bool _driver_enabled(enum DriverType driver_type); diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h index 68c0959cccec6..954b3999720b0 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.h +++ b/libraries/AP_Compass/AP_Compass_Backend.h @@ -75,6 +75,7 @@ class AP_Compass_Backend DEVTYPE_AK09915 = 0x15, DEVTYPE_QMC5883P = 0x16, DEVTYPE_BMM350 = 0x17, + DEVTYPE_IIS2MDC = 0x18, }; #if AP_COMPASS_MSP_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp new file mode 100644 index 0000000000000..6285b3c99c394 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp @@ -0,0 +1,175 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * https://www.st.com/resource/en/datasheet/iis2mdc.pdf + * + */ +#include "AP_Compass_config.h" + +#ifdef AP_COMPASS_IIS2MDC_ENABLED + +#include "AP_Compass_IIS2MDC.h" + +#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 + +// 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 const AP_HAL::HAL &hal; + +AP_Compass_Backend *AP_Compass_IIS2MDC::probe(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) +{ + if (!dev) { + return nullptr; + } + + AP_Compass_IIS2MDC *sensor = NEW_NOTHROW AP_Compass_IIS2MDC(std::move(dev),force_external,rotation); + + if (!sensor || !sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +AP_Compass_IIS2MDC::AP_Compass_IIS2MDC(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation) + : _dev(std::move(dev)) + , _rotation(rotation) + , _force_external(force_external) +{ +} + +bool AP_Compass_IIS2MDC::init() +{ + WITH_SEMAPHORE(_dev->get_semaphore()); + + _dev->set_retries(10); + + if (!check_whoami()) { + return false; + } + + if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN)) { + return false; + } + + if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC)) { + return false; + } + + if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_C, BDU)) { + return false; + } + + // lower retries for run + _dev->set_retries(3); + + // register compass instance + _dev->set_device_type(DEVTYPE_IIS2MDC); + + if (!register_compass(_dev->get_bus_id(), _instance)) { + return false; + } + + set_dev_id(_instance, _dev->get_bus_id()); + + set_rotation(_instance, _rotation); + + if (_force_external) { + set_external(_instance, true); + } + + // Enable 100HZ + _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_IIS2MDC::timer, void)); + + return true; +} + +bool AP_Compass_IIS2MDC::check_whoami() +{ + uint8_t whoami = 0; + if (!_dev->read_registers(IIS2MDC_ADDR_WHO_AM_I, &whoami, 1)){ + return false; + } + + return whoami == IIS2MDC_WHO_AM_I; +} + +void AP_Compass_IIS2MDC::timer() +{ + struct PACKED { + uint8_t xout0; + uint8_t xout1; + uint8_t yout0; + uint8_t yout1; + uint8_t zout0; + uint8_t zout1; + uint8_t tout0; + uint8_t tout1; + } buffer; + + const float range_scale = 100.f / 65.535f; // +/- 50,000 milligauss, 16bit + + uint8_t status = 0; + if (!_dev->read_registers(IIS2MDC_ADDR_STATUS_REG, &status, 1)) { + return; + } + + if (!(status & IIS2MDC_STATUS_REG_READY)) { + return; + } + + if (!_dev->read_registers(IIS2MDC_ADDR_OUTX_L_REG, (uint8_t *) &buffer, sizeof(buffer))) { + return; + } + + const int16_t x = ((buffer.xout1 << 8) | buffer.xout0); + const int16_t y = ((buffer.yout1 << 8) | buffer.yout0); + const int16_t z = -1 * ((buffer.zout1 << 8) | buffer.zout0); + + Vector3f field{ x * range_scale, y * range_scale, z * range_scale }; + + accumulate_sample(field, _instance); +} + +void AP_Compass_IIS2MDC::read() +{ + drain_accumulated_samples(_instance); +} + +#endif //AP_COMPASS_IIS2MDC_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_IIS2MDC.h b/libraries/AP_Compass/AP_Compass_IIS2MDC.h new file mode 100644 index 0000000000000..e7f062d572c70 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.h @@ -0,0 +1,68 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + */ +#pragma once + +#include +#include +#include + +#include "AP_Compass_config.h" + +#ifdef AP_COMPASS_IIS2MDC_ENABLED + +#include "AP_Compass.h" +#include "AP_Compass_Backend.h" + +#ifndef HAL_COMPASS_IIS2MDC_I2C_ADDR +#define HAL_COMPASS_IIS2MDC_I2C_ADDR 0x1E +#endif + +#ifndef HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL +#define HAL_COMPASS_IIS2MDC_ORIENTATION_EXTERNAL ROTATION_NONE +#endif + +#ifndef HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL +#define HAL_COMPASS_IIS2MDC_ORIENTATION_INTERNAL ROTATION_NONE +#endif + +class AP_Compass_IIS2MDC : public AP_Compass_Backend +{ +public: + static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation); + + void read() override; + + static constexpr const char *name = "IIS2MDC"; + +private: + AP_Compass_IIS2MDC(AP_HAL::OwnPtr dev, + bool force_external, + enum Rotation rotation); + + bool check_whoami(); + void timer(); + bool init(); + + AP_HAL::OwnPtr _dev; + + enum Rotation _rotation; + uint8_t _instance; + bool _force_external; +}; + +#endif // AP_COMPASS_IIS2MDC_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index 9d135be9dda0c..d73d4c8781bf1 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -88,6 +88,10 @@ #define AP_COMPASS_ICM20948_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_COMPASS_IIS2MDC_ENABLED +#define AP_COMPASS_IIS2MDC_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_COMPASS_IST8308_ENABLED #define AP_COMPASS_IST8308_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED #endif