Skip to content

Commit 84187c7

Browse files
bkleinerbkleiner
committed
add USE_MAG_LIS2MDL
1 parent 3a001d0 commit 84187c7

11 files changed

Lines changed: 190 additions & 4 deletions

File tree

‎docs/development/msp/inav_enums.json‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2243,7 +2243,8 @@
22432243
"MAG_RM3100": "13",
22442244
"MAG_VCM5883": "14",
22452245
"MAG_MLX90393": "15",
2246-
"MAG_FAKE": "16",
2246+
"MAG_LIS2MDL": "16",
2247+
"MAG_FAKE": "17",
22472248
"MAG_MAX": "MAG_FAKE"
22482249
},
22492250
"mavlinkAutopilotType_e": {
@@ -4134,4 +4135,4 @@
41344135
"ZERO_CALIBRATION_DONE": "2",
41354136
"ZERO_CALIBRATION_FAIL": "3"
41364137
}
4137-
}
4138+
}

‎docs/development/msp/inav_enums_ref.md‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3379,7 +3379,8 @@
33793379
| `MAG_RM3100` | 13 | |
33803380
| `MAG_VCM5883` | 14 | |
33813381
| `MAG_MLX90393` | 15 | |
3382-
| `MAG_FAKE` | 16 | |
3382+
| `MAG_LIS2MDL` | 16 | |
3383+
| `MAG_FAKE` | 17 | |
33833384
| `MAG_MAX` | MAG_FAKE | |
33843385

33853386
---

‎src/main/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,8 @@ main_sources(COMMON_SRC
146146
drivers/compass/compass_ist8308.h
147147
drivers/compass/compass_ist8310.c
148148
drivers/compass/compass_ist8310.h
149+
drivers/compass/compass_lis2mdl.c
150+
drivers/compass/compass_lis2mdl.h
149151
drivers/compass/compass_lis3mdl.c
150152
drivers/compass/compass_lis3mdl.h
151153
drivers/compass/compass_mag3110.c

‎src/main/drivers/bus.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,7 @@ typedef enum {
149149
DEVHW_SDCARD, // Generic SD-Card
150150
DEVHW_IRLOCK, // IR-Lock visual positioning hardware
151151
DEVHW_PCF8574, // 8-bit I/O expander
152+
DEVHW_LIS2MDL, // Compass; appended to preserve existing hardware IDs
152153
} devHardwareType_e;
153154

154155
typedef enum {

‎src/main/drivers/compass/compass_lis2mdl.c‎

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
/*
2+
* This file is part of INAV Project.
3+
*
4+
* INAV Project is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* INAV Project is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
* General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see http://www.gnu.org/licenses/.
16+
*/
17+
18+
#include <stdbool.h>
19+
#include <stdint.h>
20+
21+
#include "platform.h"
22+
23+
#ifdef USE_MAG_LIS2MDL
24+
25+
#include "common/axis.h"
26+
27+
#include "drivers/time.h"
28+
#include "drivers/sensor.h"
29+
30+
#include "drivers/compass/compass.h"
31+
#include "drivers/compass/compass_lis2mdl.h"
32+
33+
// LIS2MDL, IIS2MDC, LSM303AGR and LSM303AH are firmware and pin-to-pin compatible solutions.
34+
35+
#define LIS2MDL_DEVICE_ID 0x40
36+
37+
#define LIS2MDL_REG_WHO_AM_I 0x4F
38+
#define LIS2MDL_REG_CFG_REG_A 0x60
39+
#define LIS2MDL_REG_CFG_REG_B 0x61
40+
#define LIS2MDL_REG_CFG_REG_C 0x62
41+
#define LIS2MDL_REG_STATUS_REG 0x67
42+
#define LIS2MDL_REG_OUTX_L 0x68
43+
44+
// CFG_REG_A
45+
#define LIS2MDL_MD_CONTINUOUS 0x00
46+
#define LIS2MDL_ODR_100HZ 0x0C
47+
#define LIS2MDL_COMP_TEMP_EN 0x80
48+
49+
// CFG_REG_B
50+
#define LIS2MDL_OFF_CANC 0x02
51+
52+
// CFG_REG_C
53+
#define LIS2MDL_BDU 0x10
54+
55+
// STATUS_REG
56+
#define LIS2MDL_STATUS_ZYXDA 0x08
57+
58+
static bool lis2mdlInit(magDev_t *mag)
59+
{
60+
bool ack = true;
61+
62+
ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_A, LIS2MDL_MD_CONTINUOUS | LIS2MDL_ODR_100HZ | LIS2MDL_COMP_TEMP_EN);
63+
ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_B, LIS2MDL_OFF_CANC);
64+
ack = ack && busWrite(mag->busDev, LIS2MDL_REG_CFG_REG_C, LIS2MDL_BDU);
65+
66+
return ack;
67+
}
68+
69+
static bool lis2mdlRead(magDev_t *mag)
70+
{
71+
uint8_t status = 0;
72+
uint8_t buf[6];
73+
74+
mag->magADCRaw[X] = 0;
75+
mag->magADCRaw[Y] = 0;
76+
mag->magADCRaw[Z] = 0;
77+
78+
bool ack = busRead(mag->busDev, LIS2MDL_REG_STATUS_REG, &status);
79+
if (!ack || !(status & LIS2MDL_STATUS_ZYXDA)) {
80+
return false;
81+
}
82+
83+
ack = busReadBuf(mag->busDev, LIS2MDL_REG_OUTX_L, buf, 6);
84+
if (!ack) {
85+
return false;
86+
}
87+
88+
const int16_t x = (int16_t)(buf[1] << 8 | buf[0]);
89+
const int16_t y = (int16_t)(buf[3] << 8 | buf[2]);
90+
const int16_t z = (int16_t)(buf[5] << 8 | buf[4]);
91+
92+
// Adapt LIS2MDL left-handed frame to common sensor axis orientation, matching Betaflight.
93+
mag->magADCRaw[X] = -x;
94+
mag->magADCRaw[Y] = y;
95+
mag->magADCRaw[Z] = z;
96+
97+
return true;
98+
}
99+
100+
#define DETECTION_MAX_RETRY_COUNT 5
101+
static bool deviceDetect(magDev_t *mag)
102+
{
103+
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
104+
delay(10);
105+
106+
uint8_t sig = 0;
107+
const bool ack = busRead(mag->busDev, LIS2MDL_REG_WHO_AM_I, &sig);
108+
109+
if (ack && sig == LIS2MDL_DEVICE_ID) {
110+
return true;
111+
}
112+
}
113+
114+
return false;
115+
}
116+
117+
bool lis2mdlDetect(magDev_t *mag)
118+
{
119+
mag->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_LIS2MDL, mag->magSensorToUse, OWNER_COMPASS);
120+
if (mag->busDev == NULL) {
121+
return false;
122+
}
123+
124+
if (!deviceDetect(mag)) {
125+
busDeviceDeInit(mag->busDev);
126+
return false;
127+
}
128+
129+
mag->init = lis2mdlInit;
130+
mag->read = lis2mdlRead;
131+
132+
return true;
133+
}
134+
135+
#endif

‎src/main/drivers/compass/compass_lis2mdl.h‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
/*
2+
* This file is part of INAV Project.
3+
*
4+
* INAV Project is free software: you can redistribute it and/or modify
5+
* it under the terms of the GNU General Public License as published by
6+
* the Free Software Foundation, either version 3 of the License, or
7+
* (at your option) any later version.
8+
*
9+
* INAV Project is distributed in the hope that it will be useful,
10+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
11+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
* General Public License for more details.
13+
*
14+
* You should have received a copy of the GNU General Public License
15+
* along with this program. If not, see http://www.gnu.org/licenses/.
16+
*/
17+
18+
#pragma once
19+
20+
#include "drivers/io_types.h"
21+
#include "drivers/compass/compass.h"
22+
23+
bool lis2mdlDetect(magDev_t *mag);

‎src/main/fc/settings.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ tables:
88
values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"]
99
enum: rangefinderType_e
1010
- name: mag_hardware
11-
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]
11+
values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "LIS2MDL", "FAKE"]
1212
enum: magSensor_e
1313
- name: opflow_hardware
1414
values: ["NONE", "CXOF", "MSP", "FAKE"]

‎src/main/sensors/compass.c‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@
4040
#include "drivers/compass/compass_qmc5883l.h"
4141
#include "drivers/compass/compass_qmc5883p.h"
4242
#include "drivers/compass/compass_mpu9250.h"
43+
#include "drivers/compass/compass_lis2mdl.h"
4344
#include "drivers/compass/compass_lis3mdl.h"
4445
#include "drivers/compass/compass_rm3100.h"
4546
#include "drivers/compass/compass_vcm5883.h"
@@ -208,6 +209,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
208209
#endif
209210
FALLTHROUGH;
210211

212+
case MAG_LIS2MDL:
213+
#ifdef USE_MAG_LIS2MDL
214+
if (lis2mdlDetect(dev)) {
215+
magHardware = MAG_LIS2MDL;
216+
break;
217+
}
218+
#endif
219+
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
220+
if (magHardwareToUse != MAG_AUTODETECT) {
221+
break;
222+
}
223+
FALLTHROUGH;
224+
211225
case MAG_LIS3MDL:
212226
#ifdef USE_MAG_LIS3MDL
213227
if (lis3mdlDetect(dev)) {

‎src/main/sensors/compass.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ typedef enum {
4444
MAG_RM3100,
4545
MAG_VCM5883,
4646
MAG_MLX90393,
47+
MAG_LIS2MDL,
4748
MAG_FAKE,
4849
MAG_MAX = MAG_FAKE
4950
} magSensor_e;

‎src/main/target/common_hardware.c‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -253,6 +253,13 @@
253253
BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG3110_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE, 0);
254254
#endif
255255

256+
#if defined(USE_MAG_LIS2MDL)
257+
#if !defined(LIS2MDL_I2C_BUS)
258+
#define LIS2MDL_I2C_BUS MAG_I2C_BUS
259+
#endif
260+
BUSDEV_REGISTER_I2C(busdev_lis2mdl, DEVHW_LIS2MDL, LIS2MDL_I2C_BUS, 0x1E, NONE, DEVFLAGS_NONE, 0);
261+
#endif
262+
256263
#if defined(USE_MAG_LIS3MDL)
257264
#if !defined(LIS3MDL_I2C_BUS)
258265
#define LIS3MDL_I2C_BUS MAG_I2C_BUS

0 commit comments

Comments
 (0)