STMicro LIS2MDL mag support (#13150)

* STMicro LIS2MDL mag support

* Fix copyright headers

* Update PG_COMPASS_CONFIG

mk/source.mk

@@ -368,6 +368,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             drivers/compass/compass_fake.c \
             drivers/compass/compass_hmc5883l.c \
             drivers/compass/compass_qmc5883l.c \
+            drivers/compass/compass_lis2mdl.c \
             drivers/compass/compass_lis3mdl.c \
             drivers/compass/compass_ist8310.c \
             drivers/display_ug2864hsweg01.c \

src/main/cli/settings.c

@@ -157,7 +157,7 @@ const char * const lookupTableBaroHardware[] = {
 #if defined(USE_SENSOR_NAMES) || defined(USE_MAG)
 // sync with magSensor_e
 const char * const lookupTableMagHardware[] = {
-    "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS3MDL", "MPU925X_AK8963", "IST8310"
+    "AUTO", "NONE", "HMC5883", "AK8975", "AK8963", "QMC5883", "LIS2MDL", "LIS3MDL", "MPU925X_AK8963", "IST8310"
 };
 #endif
 #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)

src/main/drivers/compass/compass_lis2mdl.c

@@ -0,0 +1,185 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#if defined(USE_MAG_LIS2MDL)
+
+#include "compass.h"
+#include "drivers/time.h"
+#include "common/axis.h"
+
+#define LIS2MDL_MAG_I2C_ADDRESS     0x1E
+
+// Macros to encode/decode multi-bit values
+#define LIS2MDL_ENCODE_BITS(val, mask, shift)   ((val << shift) & mask)
+#define LIS2MDL_DECODE_BITS(val, mask, shift)   ((val & mask) >> shift)
+
+#define LIS2MDL_OFFSET_X_REG_L      0x45
+#define LIS2MDL_OFFSET_X_REG_H      0x46
+#define LIS2MDL_OFFSET_Y_REG_L      0x47
+#define LIS2MDL_OFFSET_Y_REG_H      0x48
+#define LIS2MDL_OFFSET_Z_REG_L      0x49
+#define LIS2MDL_OFFSET_Z_REG_H      0x4A
+
+#define LIS2MDL_REG_WHO_AM_I        0x4F
+#define LIS2MDL_DEVICE_ID                   0x40
+
+#define LIS2MDL_CFG_REG_A           0x60
+#define LIS2MDL_CFG_REG_A_COMP_TEMP_EN      0x80
+#define LIS2MDL_CFG_REG_A_REBOOT            0x40
+#define LIS2MDL_CFG_REG_A_SOFT_RST          0x20
+#define LIS2MDL_CFG_REG_A_LP                0x10
+#define LIS2MDL_CFG_REG_A_ODR_MASK          0x0c
+#define LIS2MDL_CFG_REG_A_ODR_SHIFT         2
+#define LIS2MDL_CFG_REG_A_ODR_10            0
+#define LIS2MDL_CFG_REG_A_ODR_20            1
+#define LIS2MDL_CFG_REG_A_ODR_50            2
+#define LIS2MDL_CFG_REG_A_ODR_100           3
+#define LIS2MDL_CFG_REG_A_MD_MASK           0x03
+#define LIS2MDL_CFG_REG_A_MD_SHIFT          0
+#define LIS2MDL_CFG_REG_A_MD_CONT           0
+#define LIS2MDL_CFG_REG_A_MD_SINGLE         1
+#define LIS2MDL_CFG_REG_A_MD_IDLE           3
+
+#define LIS2MDL_CFG_REG_B           0x61
+#define LIS2MDL_CFG_REG_B_OFF_CANC_ONE_SHOT 0x10
+#define LIS2MDL_CFG_REG_B_INT_ON_DATA_OFF   0x08
+#define LIS2MDL_CFG_REG_B_SET_FREQ          0x04
+#define LIS2MDL_CFG_REG_B_OFF_CANC          0x02
+#define LIS2MDL_CFG_REG_B_LPF               0x01
+
+#define LIS2MDL_CFG_REG_C           0x62
+#define LIS2MDL_CFG_REG_C_INT_ON_PIN        0x40
+#define LIS2MDL_CFG_REG_C_I2C_DIS           0x20
+#define LIS2MDL_CFG_REG_C_BDU               0x10
+#define LIS2MDL_CFG_REG_C_BLE               0x08
+#define LIS2MDL_CFG_REG_C_4WSPI             0x04
+#define LIS2MDL_CFG_REG_C_SELF_TEST         0x02
+#define LIS2MDL_CFG_REG_C_DRDY_ON_PIN       0x01
+
+#define LIS2MDL_INT_CTRL_REG        0x63
+#define LIS2MDL_INT_CTRL_REG_XIEN           0x80
+#define LIS2MDL_INT_CTRL_REG_YIEN           0x40
+#define LIS2MDL_INT_CTRL_REG_ZIEN           0x20
+#define LIS2MDL_INT_CTRL_REG_IEA            0x04
+#define LIS2MDL_INT_CTRL_REG_IEL            0x02
+#define LIS2MDL_INT_CTRL_REG_IEN            0x01
+
+#define LIS2MDL_INT_SOURCE_REG      0x64
+#define LIS2MDL_INT_SOURCE_REG_P_TH_S_X     0x80
+#define LIS2MDL_INT_SOURCE_REG_P_TH_S_Y     0x40
+#define LIS2MDL_INT_SOURCE_REG_P_TH_S_Z     0x20
+#define LIS2MDL_INT_SOURCE_REG_N_TH_S_X     0x10
+#define LIS2MDL_INT_SOURCE_REG_N_TH_S_Y     0x08
+#define LIS2MDL_INT_SOURCE_REG_N_TH_S_Z     0x04
+#define LIS2MDL_INT_SOURCE_REG_MROI         0x02
+#define LIS2MDL_INT_SOURCE_REG_INT          0x01
+
+#define LIS2MDL_INT_THS_L_REG       0x65
+#define LIS2MDL_INT_THS_H_REG       0x66
+
+#define LIS2MDL_STATUS_REG          0x67
+#define LIS2MDL_STATUS_REG_ZXYOR            0x80
+#define LIS2MDL_STATUS_REG_ZOR              0x40
+#define LIS2MDL_STATUS_REG_YOR              0x20
+#define LIS2MDL_STATUS_REG_XOR              0x10
+#define LIS2MDL_STATUS_REG_ZXYDA            0x08
+#define LIS2MDL_STATUS_REG_ZDA              0x04
+#define LIS2MDL_STATUS_REG_YDA              0x02
+#define LIS2MDL_STATUS_REG_XDA              0x01
+
+#define LIS2MDL_OUTX_L_REG          0x68
+#define LIS2MDL_OUTX_H_REG          0x69
+#define LIS2MDL_OUTY_L_REG          0x6A
+#define LIS2MDL_OUTY_H_REG          0x6B
+#define LIS2MDL_OUTZ_L_REG          0x6C
+#define LIS2MDL_OUTZ_H_REG          0x6D
+
+#define LIS2MDL_TEMP_OUT_L_REG      0x6E
+#define LIS2MDL_TEMP_OUT_H_REG      0x6F
+
+static bool lis2mdlRead(magDev_t * mag, int16_t *magData)
+{
+    static uint8_t buf[6];
+    static bool pendingRead = true;
+
+    extDevice_t *dev = &mag->dev;
+
+    if (pendingRead) {
+        busReadRegisterBufferStart(dev, LIS2MDL_OUTX_L_REG, buf, sizeof(buf));
+
+        pendingRead = false;
+        return false;
+    }
+
+    magData[X] = (int16_t)(buf[1] << 8 | buf[0]);
+    magData[Y] = (int16_t)(buf[3] << 8 | buf[2]);
+    magData[Z] = (int16_t)(buf[5] << 8 | buf[4]);
+
+    pendingRead = true;
+
+    return true;
+}
+
+static bool lis2mdlInit(magDev_t *mag)
+{
+    extDevice_t *dev = &mag->dev;
+
+    busDeviceRegister(dev);
+
+    busWriteRegister(dev, LIS2MDL_CFG_REG_A,
+                     LIS2MDL_CFG_REG_A_COMP_TEMP_EN |
+                     LIS2MDL_ENCODE_BITS(LIS2MDL_CFG_REG_A_ODR_100, LIS2MDL_CFG_REG_A_ODR_MASK, LIS2MDL_CFG_REG_A_ODR_SHIFT) |
+                     LIS2MDL_ENCODE_BITS(LIS2MDL_CFG_REG_A_MD_CONT, LIS2MDL_CFG_REG_A_MD_MASK, LIS2MDL_CFG_REG_A_MD_SHIFT));
+
+    delay(100);
+
+    return true;
+}
+
+bool lis2mdlDetect(magDev_t * mag)
+{
+    extDevice_t *dev = &mag->dev;
+
+    uint8_t sig = 0;
+
+    if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
+        dev->busType_u.i2c.address = LIS2MDL_MAG_I2C_ADDRESS;
+    }
+
+    bool ack = busReadRegisterBuffer(&mag->dev, LIS2MDL_REG_WHO_AM_I, &sig, 1);
+
+    if (!ack || sig != LIS2MDL_DEVICE_ID) {
+        return false;
+    }
+
+    mag->init = lis2mdlInit;
+    mag->read = lis2mdlRead;
+
+    return true;
+}
+#endif

src/main/drivers/compass/compass_lis2mdl.h

@@ -0,0 +1,26 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software 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.
+ *
+ * Betaflight 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/io_types.h"
+
+bool lis2mdlDetect(magDev_t* mag);

src/main/sensors/compass.c

@@ -43,6 +43,7 @@
 #include "drivers/compass/compass_ak8963.h"
 #include "drivers/compass/compass_virtual.h"
 #include "drivers/compass/compass_hmc5883l.h"
+#include "drivers/compass/compass_lis2mdl.h"
 #include "drivers/compass/compass_lis3mdl.h"
 #include "drivers/compass/compass_mpu925x_ak8963.h"
 #include "drivers/compass/compass_qmc5883l.h"
@@ -86,7 +87,7 @@ static compassBiasEstimator_t compassBiasEstimator;
 magDev_t magDev;
 mag_t mag;
 
-PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
+PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4);
 
 // If the i2c bus is busy, try again in 500us
 #define COMPASS_BUS_BUSY_INTERVAL_US 500
@@ -226,6 +227,22 @@ bool compassDetect(magDev_t *magDev, uint8_t *alignment)
 #endif
         FALLTHROUGH;
 
+    case MAG_LIS2MDL:
+#if defined(USE_MAG_LIS2MDL)
+        if (dev->bus->busType == BUS_TYPE_I2C) {
+            dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
+        }
+
+        if (lis2mdlDetect(magDev)) {
+#ifdef MAG_LIS3MDL_ALIGN
+            *alignment = MAG_LIS2MDL_ALIGN;
+#endif
+            magHardware = MAG_LIS2MDL;
+            break;
+        }
+#endif
+        FALLTHROUGH;
+
     case MAG_LIS3MDL:
 #if defined(USE_MAG_LIS3MDL)
         if (dev->bus->busType == BUS_TYPE_I2C) {

src/main/sensors/compass.h

@@ -37,9 +37,10 @@ typedef enum {
     MAG_AK8975 = 3,
     MAG_AK8963 = 4,
     MAG_QMC5883 = 5,
-    MAG_LIS3MDL = 6,
-    MAG_MPU925X_AK8963 = 7,
-    MAG_IST8310 = 8
+    MAG_LIS2MDL = 6,
+    MAG_LIS3MDL = 7,
+    MAG_MPU925X_AK8963 = 8,
+    MAG_IST8310 = 9
 } magSensor_e;
 
 typedef struct mag_s {

src/main/target/common_post.h

@@ -99,6 +99,9 @@
 #ifndef USE_MAG_QMC5883
 #define USE_MAG_QMC5883
 #endif
+#ifndef USE_MAG_LIS2MDL
+#define USE_MAG_LIS2MDL
+#endif
 #ifndef USE_MAG_LIS3MDL
 #define USE_MAG_LIS3MDL
 #endif