From 7f4c45a2bb7bb683982fe65a81e11153a487e263 Mon Sep 17 00:00:00 2001 From: Benedikt Kleiner Date: Mon, 17 Aug 2026 12:02:33 +0800 Subject: [PATCH] compass: add LIS2MDL support --- docs/Settings.md | 1 + docs/development/msp/inav_enums.json | 54 +++++---- docs/development/msp/inav_enums_ref.md | 54 +++++---- src/main/CMakeLists.txt | 2 + src/main/drivers/bus.h | 1 + src/main/drivers/compass/compass_lis2mdl.c | 135 +++++++++++++++++++++ src/main/drivers/compass/compass_lis2mdl.h | 23 ++++ src/main/fc/settings.yaml | 2 +- src/main/sensors/compass.c | 20 ++- src/main/sensors/compass.h | 1 + src/main/target/common_hardware.c | 7 ++ src/main/target/common_post.h | 1 + 12 files changed, 247 insertions(+), 54 deletions(-) create mode 100644 src/main/drivers/compass/compass_lis2mdl.c create mode 100644 src/main/drivers/compass/compass_lis2mdl.h diff --git a/docs/Settings.md b/docs/Settings.md index c8eb8bb0716..c8758aacbc0 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2780,6 +2780,7 @@ Selection of mag hardware. See Wiki Sensor auto detect and hardware failure dete | RM3100 | | | VCM5883 | | | MLX90393 | | +| LIS2MDL | | | FAKE | | --- diff --git a/docs/development/msp/inav_enums.json b/docs/development/msp/inav_enums.json index 52241112fd3..73a25ca4458 100644 --- a/docs/development/msp/inav_enums.json +++ b/docs/development/msp/inav_enums.json @@ -842,31 +842,32 @@ "DEVHW_RM3100": "31", "DEVHW_VCM5883": "32", "DEVHW_MLX90393": "33", - "DEVHW_LM75_0": "34", - "DEVHW_LM75_1": "35", - "DEVHW_LM75_2": "36", - "DEVHW_LM75_3": "37", - "DEVHW_LM75_4": "38", - "DEVHW_LM75_5": "39", - "DEVHW_LM75_6": "40", - "DEVHW_LM75_7": "41", - "DEVHW_DS2482": "42", - "DEVHW_MAX7456": "43", - "DEVHW_SRF10": "44", - "DEVHW_VL53L0X": "45", - "DEVHW_VL53L1X": "46", - "DEVHW_US42": "47", - "DEVHW_TOF10120_I2C": "48", - "DEVHW_TERARANGER_EVO_I2C": "49", - "DEVHW_MS4525": "50", - "DEVHW_MS5525": "51", - "DEVHW_DLVR": "52", - "DEVHW_M25P16": "53", - "DEVHW_W25N": "54", - "DEVHW_UG2864": "55", - "DEVHW_SDCARD": "56", - "DEVHW_IRLOCK": "57", - "DEVHW_PCF8574": "58" + "DEVHW_LIS2MDL": "34", + "DEVHW_LM75_0": "35", + "DEVHW_LM75_1": "36", + "DEVHW_LM75_2": "37", + "DEVHW_LM75_3": "38", + "DEVHW_LM75_4": "39", + "DEVHW_LM75_5": "40", + "DEVHW_LM75_6": "41", + "DEVHW_LM75_7": "42", + "DEVHW_DS2482": "43", + "DEVHW_MAX7456": "44", + "DEVHW_SRF10": "45", + "DEVHW_VL53L0X": "46", + "DEVHW_VL53L1X": "47", + "DEVHW_US42": "48", + "DEVHW_TOF10120_I2C": "49", + "DEVHW_TERARANGER_EVO_I2C": "50", + "DEVHW_MS4525": "51", + "DEVHW_MS5525": "52", + "DEVHW_DLVR": "53", + "DEVHW_M25P16": "54", + "DEVHW_W25N": "55", + "DEVHW_UG2864": "56", + "DEVHW_SDCARD": "57", + "DEVHW_IRLOCK": "58", + "DEVHW_PCF8574": "59" }, "deviceFlags_e": { "_source": "inav/src/main/drivers/bus.h", @@ -2275,7 +2276,8 @@ "MAG_RM3100": "13", "MAG_VCM5883": "14", "MAG_MLX90393": "15", - "MAG_FAKE": "16", + "MAG_LIS2MDL": "16", + "MAG_FAKE": "17", "MAG_MAX": "MAG_FAKE" }, "mavlinkAutopilotType_e": { diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md index 275145df9f6..83724ada7cf 100644 --- a/docs/development/msp/inav_enums_ref.md +++ b/docs/development/msp/inav_enums_ref.md @@ -1444,31 +1444,32 @@ | `DEVHW_RM3100` | 31 | | | `DEVHW_VCM5883` | 32 | | | `DEVHW_MLX90393` | 33 | | -| `DEVHW_LM75_0` | 34 | | -| `DEVHW_LM75_1` | 35 | | -| `DEVHW_LM75_2` | 36 | | -| `DEVHW_LM75_3` | 37 | | -| `DEVHW_LM75_4` | 38 | | -| `DEVHW_LM75_5` | 39 | | -| `DEVHW_LM75_6` | 40 | | -| `DEVHW_LM75_7` | 41 | | -| `DEVHW_DS2482` | 42 | | -| `DEVHW_MAX7456` | 43 | | -| `DEVHW_SRF10` | 44 | | -| `DEVHW_VL53L0X` | 45 | | -| `DEVHW_VL53L1X` | 46 | | -| `DEVHW_US42` | 47 | | -| `DEVHW_TOF10120_I2C` | 48 | | -| `DEVHW_TERARANGER_EVO_I2C` | 49 | | -| `DEVHW_MS4525` | 50 | | -| `DEVHW_MS5525` | 51 | | -| `DEVHW_DLVR` | 52 | | -| `DEVHW_M25P16` | 53 | | -| `DEVHW_W25N` | 54 | | -| `DEVHW_UG2864` | 55 | | -| `DEVHW_SDCARD` | 56 | | -| `DEVHW_IRLOCK` | 57 | | -| `DEVHW_PCF8574` | 58 | | +| `DEVHW_LIS2MDL` | 34 | | +| `DEVHW_LM75_0` | 35 | | +| `DEVHW_LM75_1` | 36 | | +| `DEVHW_LM75_2` | 37 | | +| `DEVHW_LM75_3` | 38 | | +| `DEVHW_LM75_4` | 39 | | +| `DEVHW_LM75_5` | 40 | | +| `DEVHW_LM75_6` | 41 | | +| `DEVHW_LM75_7` | 42 | | +| `DEVHW_DS2482` | 43 | | +| `DEVHW_MAX7456` | 44 | | +| `DEVHW_SRF10` | 45 | | +| `DEVHW_VL53L0X` | 46 | | +| `DEVHW_VL53L1X` | 47 | | +| `DEVHW_US42` | 48 | | +| `DEVHW_TOF10120_I2C` | 49 | | +| `DEVHW_TERARANGER_EVO_I2C` | 50 | | +| `DEVHW_MS4525` | 51 | | +| `DEVHW_MS5525` | 52 | | +| `DEVHW_DLVR` | 53 | | +| `DEVHW_M25P16` | 54 | | +| `DEVHW_W25N` | 55 | | +| `DEVHW_UG2864` | 56 | | +| `DEVHW_SDCARD` | 57 | | +| `DEVHW_IRLOCK` | 58 | | +| `DEVHW_PCF8574` | 59 | | --- ## `deviceFlags_e` @@ -3417,7 +3418,8 @@ | `MAG_RM3100` | 13 | | | `MAG_VCM5883` | 14 | | | `MAG_MLX90393` | 15 | | -| `MAG_FAKE` | 16 | | +| `MAG_LIS2MDL` | 16 | | +| `MAG_FAKE` | 17 | | | `MAG_MAX` | MAG_FAKE | | --- diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f4ab6479a5f..f28a5df266b 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -148,6 +148,8 @@ main_sources(COMMON_SRC drivers/compass/compass_ist8308.h drivers/compass/compass_ist8310.c drivers/compass/compass_ist8310.h + drivers/compass/compass_lis2mdl.c + drivers/compass/compass_lis2mdl.h drivers/compass/compass_lis3mdl.c drivers/compass/compass_lis3mdl.h drivers/compass/compass_mag3110.c diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 385fbd36c0e..7bb457cb00b 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -114,6 +114,7 @@ typedef enum { DEVHW_RM3100, DEVHW_VCM5883, DEVHW_MLX90393, + DEVHW_LIS2MDL, /* Temp sensor chips */ DEVHW_LM75_0, diff --git a/src/main/drivers/compass/compass_lis2mdl.c b/src/main/drivers/compass/compass_lis2mdl.c new file mode 100644 index 00000000000..625d0c4de50 --- /dev/null +++ b/src/main/drivers/compass/compass_lis2mdl.c @@ -0,0 +1,135 @@ +/* + * This file is part of INAV Project. + * + * INAV Project 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. + * + * INAV Project 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/. + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_MAG_LIS2MDL + +#include "common/axis.h" + +#include "drivers/time.h" +#include "drivers/sensor.h" + +#include "drivers/compass/compass.h" +#include "drivers/compass/compass_lis2mdl.h" + +// LIS2MDL, IIS2MDC, LSM303AGR and LSM303AH are firmware and pin-to-pin compatible solutions. + +#define LIS2MDL_DEVICE_ID 0x40 + +#define LIS2MDL_REG_WHO_AM_I 0x4F +#define LIS2MDL_REG_CFG_REG_A 0x60 +#define LIS2MDL_REG_CFG_REG_B 0x61 +#define LIS2MDL_REG_CFG_REG_C 0x62 +#define LIS2MDL_REG_STATUS_REG 0x67 +#define LIS2MDL_REG_OUTX_L 0x68 + +// CFG_REG_A +#define LIS2MDL_MD_CONTINUOUS 0x00 +#define LIS2MDL_ODR_100HZ 0x0C +#define LIS2MDL_COMP_TEMP_EN 0x80 + +// CFG_REG_B +#define LIS2MDL_OFF_CANC 0x02 + +// CFG_REG_C +#define LIS2MDL_BDU 0x10 + +// STATUS_REG +#define LIS2MDL_STATUS_ZYXDA 0x08 + +static bool lis2mdlInit(magDev_t *mag) +{ + bool ack = true; + + ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_A, LIS2MDL_MD_CONTINUOUS | LIS2MDL_ODR_100HZ | LIS2MDL_COMP_TEMP_EN); + ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_B, LIS2MDL_OFF_CANC); + ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_C, LIS2MDL_BDU); + + return ack; +} + +static bool lis2mdlRead(magDev_t *mag) +{ + uint8_t status = 0; + uint8_t buf[6]; + + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; + + bool ack = busRead(mag->busDev, LIS2MDL_REG_STATUS_REG, &status); + if (!ack || !(status & LIS2MDL_STATUS_ZYXDA)) { + return false; + } + + ack = busReadBuf(mag->busDev, LIS2MDL_REG_OUTX_L, buf, 6); + if (!ack) { + return false; + } + + const int16_t x = (int16_t)(buf[1] << 8 | buf[0]); + const int16_t y = (int16_t)(buf[3] << 8 | buf[2]); + const int16_t z = (int16_t)(buf[5] << 8 | buf[4]); + + // Adapt LIS2MDL left-handed frame to common sensor axis orientation, matching Betaflight. + mag->magADCRaw[X] = -x; + mag->magADCRaw[Y] = y; + mag->magADCRaw[Z] = z; + + return true; +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(magDev_t *mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + delay(10); + + uint8_t sig = 0; + const bool ack = busRead(mag->busDev, LIS2MDL_REG_WHO_AM_I, &sig); + + if (ack && sig == LIS2MDL_DEVICE_ID) { + return true; + } + } + + return false; +} + +bool lis2mdlDetect(magDev_t *mag) +{ + mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LIS2MDL, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = lis2mdlInit; + mag->read = lis2mdlRead; + + return true; +} + +#endif diff --git a/src/main/drivers/compass/compass_lis2mdl.h b/src/main/drivers/compass/compass_lis2mdl.h new file mode 100644 index 00000000000..1d4ca62e082 --- /dev/null +++ b/src/main/drivers/compass/compass_lis2mdl.h @@ -0,0 +1,23 @@ +/* + * This file is part of INAV Project. + * + * INAV Project 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. + * + * INAV Project 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 "drivers/io_types.h" +#include "drivers/compass/compass.h" + +bool lis2mdlDetect(magDev_t *mag); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 74ad48e8d58..164f238aef2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -8,7 +8,7 @@ tables: values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] enum: rangefinderType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "LIS2MDL", "FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "CXOF", "MSP", "FAKE"] diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 4e87b944168..bd61b2d8c38 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,6 +40,7 @@ #include "drivers/compass/compass_qmc5883l.h" #include "drivers/compass/compass_qmc5883p.h" #include "drivers/compass/compass_mpu9250.h" +#include "drivers/compass/compass_lis2mdl.h" #include "drivers/compass/compass_lis3mdl.h" #include "drivers/compass/compass_rm3100.h" #include "drivers/compass/compass_vcm5883.h" @@ -65,7 +66,7 @@ mag_t mag; // mag access functions #ifdef USE_MAG -PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 7); PG_RESET_TEMPLATE(compassConfig_t, compassConfig, .mag_align = SETTING_ALIGN_MAG_DEFAULT, @@ -206,6 +207,23 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) break; } #endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + + case MAG_LIS2MDL: +#ifdef USE_MAG_LIS2MDL + if (lis2mdlDetect(dev)) { + magHardware = MAG_LIS2MDL; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } FALLTHROUGH; case MAG_LIS3MDL: diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 17930678f58..f172f1601e2 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -44,6 +44,7 @@ typedef enum { MAG_RM3100, MAG_VCM5883, MAG_MLX90393, + MAG_LIS2MDL, MAG_FAKE, MAG_MAX = MAG_FAKE } magSensor_e; diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 8314926184f..fe8b6f1cd07 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -253,6 +253,13 @@ BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE, 0); #endif +#if defined(USE_MAG_LIS2MDL) + #if !defined(LIS2MDL_I2C_BUS) + #define LIS2MDL_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_lis2mdl, DEVHW_LIS2MDL, LIS2MDL_I2C_BUS, 0x1E, NONE, DEVFLAGS_NONE, 0); +#endif + #if defined(USE_MAG_LIS3MDL) #if !defined(LIS3MDL_I2C_BUS) #define LIS3MDL_I2C_BUS MAG_I2C_BUS diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 5d262ad48f5..2a85dbdc97a 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -82,6 +82,7 @@ extern uint8_t __config_end; #define USE_MAG_HMC5883 #define USE_MAG_IST8310 +#define USE_MAG_LIS2MDL #define USE_MAG_LIS3MDL #define USE_MAG_MAG3110 #define USE_MAG_QMC5883