AP_Compass: add IIS2MDC driver

Tools/scripts/build_options.py

@@ -171,6 +171,7 @@ BUILD_OPTIONS = [
     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),

Tools/scripts/decode_devid.py

@@ -67,6 +67,7 @@ compass_types = {
     0x15 : "DEVTYPE_AK09915",
     0x16 : "DEVTYPE_QMC5883P",
     0x17 : "DEVTYPE_BMM350",
+    0x18 : "DEVTYPE_IIS2MDC",
 }
 
 imu_types = {

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

libraries/AP_Compass/AP_Compass.h

@@ -499,6 +499,9 @@ private:
 #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);

libraries/AP_Compass/AP_Compass_Backend.h

@@ -75,6 +75,7 @@ public:
         DEVTYPE_AK09915 = 0x15,
     	DEVTYPE_QMC5883P = 0x16,
         DEVTYPE_BMM350 = 0x17,
+        DEVTYPE_IIS2MDC = 0x18,
     };
 
 #if AP_COMPASS_MSP_ENABLED

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 <http://www.gnu.org/licenses/>.
+ *
+ * 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 <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+// 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<AP_HAL::I2CDevice> 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<AP_HAL::Device> 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

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 <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+#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<AP_HAL::I2CDevice> dev,
+                                     bool force_external,
+                                     enum Rotation rotation);
+
+    void read() override;
+
+    static constexpr const char *name = "IIS2MDC";
+
+private:
+    AP_Compass_IIS2MDC(AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                        bool force_external,
+                        enum Rotation rotation);
+
+    bool check_whoami();
+    void timer();
+    bool init();
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+    enum Rotation _rotation;
+    uint8_t _instance;
+    bool _force_external;
+};
+
+#endif  // AP_COMPASS_IIS2MDC_ENABLED

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