diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index 3f5dd534ff8f1..403a49cc835fa 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -170,6 +170,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 ee80f080ae0c8..8910be0d8a2a4 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..14343721773b6
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_IIS2MDC.cpp
@@ -0,0 +1,181 @@
+/*
+ * 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 .
+ *
+ */
+#include "AP_Compass_IIS2MDC.h"
+
+#include
+
+#include
+#include
+
+#if AP_COMPASS_IIS2MDC_ENABLED
+
+// 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()
+{
+ _dev->get_semaphore()->take_blocking();
+
+ _dev->set_retries(10);
+
+ if (!check_whoami()) {
+ goto fail;
+ }
+
+ if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN)) {
+ goto fail;
+ }
+
+ if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC)) {
+ goto fail;
+ }
+
+ if (!_dev->write_register(IIS2MDC_ADDR_CFG_REG_C, BDU)) {
+ goto fail;
+ }
+
+ // lower retries for run
+ _dev->set_retries(3);
+
+ _dev->get_semaphore()->give();
+
+ // 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());
+
+ printf("%s found on bus %u id %u address 0x%02x\n", name,
+ _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address());
+
+ 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;
+
+fail:
+ _dev->get_semaphore()->give();
+ return false;
+}
+
+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 / 65535.f; // +/- 50 Gauss, 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;
+ }
+
+ int16_t x = int16_t((buffer.xout1 << 8) | buffer.xout0);
+ int16_t y = int16_t((buffer.yout1 << 8) | buffer.yout0);
+ int16_t z = -int16_t((buffer.zout1 << 8) | buffer.zout0);
+
+ Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale };
+
+ accumulate_sample(field, _instance, 20);
+}
+
+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..310fa0adab3e5
--- /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:1;
+};
+
+#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