2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #include "build/atomic.h"
29 #include "build/build_config.h"
30 #include "build/debug.h"
32 #include "common/maths.h"
33 #include "common/utils.h"
35 #include "drivers/bus.h"
36 #include "drivers/bus_i2c.h"
37 #include "drivers/bus_spi.h"
38 #include "drivers/exti.h"
39 #include "drivers/io.h"
40 #include "drivers/nvic.h"
41 #include "drivers/sensor.h"
42 #include "drivers/system.h"
43 #include "drivers/time.h"
45 #include "drivers/accgyro/accgyro.h"
46 #include "drivers/accgyro/accgyro_mpu3050.h"
47 #include "drivers/accgyro/accgyro_mpu6050.h"
48 #include "drivers/accgyro/accgyro_mpu6500.h"
49 #include "drivers/accgyro/accgyro_spi_bmi160.h"
50 #include "drivers/accgyro/accgyro_spi_bmi270.h"
51 #include "drivers/accgyro/accgyro_spi_icm20649.h"
52 #include "drivers/accgyro/accgyro_spi_icm20689.h"
53 #include "drivers/accgyro/accgyro_spi_icm426xx.h"
54 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
55 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
56 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
57 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
58 #include "drivers/accgyro/accgyro_spi_l3gd20.h"
59 #include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h"
60 #include "drivers/accgyro/accgyro_mpu.h"
63 #include "pg/gyrodev.h"
66 #define MPU_ADDRESS 0x68
69 // 1 MHz max SPI frequency during device detection
70 #define MPU_MAX_SPI_DETECT_CLK_HZ 1000000
72 #define MPU_INQUIRY_MASK 0x7E
74 // Allow 100ms before attempting to access SPI bus
75 #define GYRO_SPI_STARTUP_MS 100
77 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
78 #define GYRO_EXTI_DETECT_THRESHOLD 1000
81 static void mpu6050FindRevision(gyroDev_t
*gyro
)
83 // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
84 // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
86 // determine product ID and revision
87 uint8_t readBuffer
[6];
88 bool ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_XA_OFFS_H
, readBuffer
, 6);
89 uint8_t revision
= ((readBuffer
[5] & 0x01) << 2) | ((readBuffer
[3] & 0x01) << 1) | (readBuffer
[1] & 0x01);
90 if (ack
&& revision
) {
91 // Congrats, these parts are better
93 gyro
->mpuDetectionResult
.resolution
= MPU_HALF_RESOLUTION
;
94 } else if (revision
== 2) {
95 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
96 } else if ((revision
== 3) || (revision
== 7)) {
97 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
99 failureMode(FAILURE_ACC_INCOMPATIBLE
);
103 ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_PRODUCT_ID
, &productId
, 1);
104 revision
= productId
& 0x0F;
105 if (!ack
|| revision
== 0) {
106 failureMode(FAILURE_ACC_INCOMPATIBLE
);
107 } else if (revision
== 4) {
108 gyro
->mpuDetectionResult
.resolution
= MPU_HALF_RESOLUTION
;
110 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
117 * Gyro interrupt service routine
120 // Called in ISR context
121 // Gyro read has just completed
122 busStatus_e
mpuIntCallback(uint32_t arg
)
124 gyroDev_t
*gyro
= (gyroDev_t
*)arg
;
125 int32_t gyroDmaDuration
= cmpTimeCycles(getCycleCounter(), gyro
->gyroLastEXTI
);
127 if (gyroDmaDuration
> gyro
->gyroDmaMaxDuration
) {
128 gyro
->gyroDmaMaxDuration
= gyroDmaDuration
;
131 gyro
->dataReady
= true;
136 static void mpuIntExtiHandler(extiCallbackRec_t
*cb
)
138 gyroDev_t
*gyro
= container_of(cb
, gyroDev_t
, exti
);
140 // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
141 // not have an associated timer
142 uint32_t nowCycles
= getCycleCounter();
143 int32_t gyroLastPeriod
= cmpTimeCycles(nowCycles
, gyro
->gyroLastEXTI
);
144 // This detects the short (~79us) EXTI interval of an MPU6xxx gyro
145 if ((gyro
->gyroShortPeriod
== 0) || (gyroLastPeriod
< gyro
->gyroShortPeriod
)) {
146 gyro
->gyroSyncEXTI
= gyro
->gyroLastEXTI
+ gyro
->gyroDmaMaxDuration
;
148 gyro
->gyroLastEXTI
= nowCycles
;
150 if (gyro
->gyroModeSPI
== GYRO_EXTI_INT_DMA
) {
151 spiSequence(&gyro
->dev
, gyro
->segments
);
154 gyro
->detectedEXTI
++;
157 static void mpuIntExtiHandler(extiCallbackRec_t
*cb
)
159 gyroDev_t
*gyro
= container_of(cb
, gyroDev_t
, exti
);
160 gyro
->dataReady
= true;
164 static void mpuIntExtiInit(gyroDev_t
*gyro
)
166 if (gyro
->mpuIntExtiTag
== IO_TAG_NONE
) {
170 const IO_t mpuIntIO
= IOGetByTag(gyro
->mpuIntExtiTag
);
172 #ifdef ENSURE_MPU_DATA_READY_IS_LOW
173 uint8_t status
= IORead(mpuIntIO
);
179 IOInit(mpuIntIO
, OWNER_GYRO_EXTI
, 0);
180 EXTIHandlerInit(&gyro
->exti
, mpuIntExtiHandler
);
181 EXTIConfig(mpuIntIO
, &gyro
->exti
, NVIC_PRIO_MPU_INT_EXTI
, IOCFG_IN_FLOATING
, BETAFLIGHT_EXTI_TRIGGER_RISING
);
182 EXTIEnable(mpuIntIO
);
185 bool mpuAccRead(accDev_t
*acc
)
189 const bool ack
= busReadRegisterBuffer(&acc
->gyro
->dev
, acc
->gyro
->accDataReg
, data
, 6);
194 acc
->ADCRaw
[X
] = (int16_t)((data
[0] << 8) | data
[1]);
195 acc
->ADCRaw
[Y
] = (int16_t)((data
[2] << 8) | data
[3]);
196 acc
->ADCRaw
[Z
] = (int16_t)((data
[4] << 8) | data
[5]);
201 bool mpuGyroRead(gyroDev_t
*gyro
)
205 const bool ack
= busReadRegisterBuffer(&gyro
->dev
, gyro
->gyroDataReg
, data
, 6);
210 gyro
->gyroADCRaw
[X
] = (int16_t)((data
[0] << 8) | data
[1]);
211 gyro
->gyroADCRaw
[Y
] = (int16_t)((data
[2] << 8) | data
[3]);
212 gyro
->gyroADCRaw
[Z
] = (int16_t)((data
[4] << 8) | data
[5]);
219 bool mpuAccReadSPI(accDev_t
*acc
)
221 switch (acc
->gyro
->gyroModeSPI
) {
223 case GYRO_EXTI_NO_INT
:
225 acc
->gyro
->dev
.txBuf
[0] = acc
->gyro
->accDataReg
| 0x80;
227 busSegment_t segments
[] = {
228 {.u
.buffers
= {NULL
, NULL
}, 7, true, NULL
},
229 {.u
.link
= {NULL
, NULL
}, 0, true, NULL
},
231 segments
[0].u
.buffers
.txData
= acc
->gyro
->dev
.txBuf
;
232 segments
[0].u
.buffers
.rxData
= &acc
->gyro
->dev
.rxBuf
[1];
234 spiSequence(&acc
->gyro
->dev
, &segments
[0]);
236 // Wait for completion
237 spiWait(&acc
->gyro
->dev
);
243 case GYRO_EXTI_INT_DMA
:
245 // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
248 // This data was read from the gyro, which is the same SPI device as the acc
249 int16_t *accData
= (int16_t *)acc
->gyro
->dev
.rxBuf
;
250 acc
->ADCRaw
[X
] = __builtin_bswap16(accData
[1]);
251 acc
->ADCRaw
[Y
] = __builtin_bswap16(accData
[2]);
252 acc
->ADCRaw
[Z
] = __builtin_bswap16(accData
[3]);
264 bool mpuGyroReadSPI(gyroDev_t
*gyro
)
266 int16_t *gyroData
= (int16_t *)gyro
->dev
.rxBuf
;
267 switch (gyro
->gyroModeSPI
) {
270 // Initialise the tx buffer to all 0xff
271 memset(gyro
->dev
.txBuf
, 0xff, 16);
273 // Check that minimum number of interrupts have been detected
275 // We need some offset from the gyro interrupts to ensure sampling after the interrupt
276 gyro
->gyroDmaMaxDuration
= 5;
277 if (gyro
->detectedEXTI
> GYRO_EXTI_DETECT_THRESHOLD
) {
278 if (spiUseDMA(&gyro
->dev
)) {
279 gyro
->dev
.callbackArg
= (uint32_t)gyro
;
280 gyro
->dev
.txBuf
[0] = gyro
->accDataReg
| 0x80;
281 gyro
->segments
[0].len
= gyro
->gyroDataReg
- gyro
->accDataReg
+ sizeof(uint8_t) + 3 * sizeof(int16_t);
282 gyro
->segments
[0].callback
= mpuIntCallback
;
283 gyro
->segments
[0].u
.buffers
.txData
= gyro
->dev
.txBuf
;
284 gyro
->segments
[0].u
.buffers
.rxData
= &gyro
->dev
.rxBuf
[1];
285 gyro
->segments
[0].negateCS
= true;
286 gyro
->gyroModeSPI
= GYRO_EXTI_INT_DMA
;
288 // Interrupts are present, but no DMA
289 gyro
->gyroModeSPI
= GYRO_EXTI_INT
;
292 gyro
->gyroModeSPI
= GYRO_EXTI_NO_INT
;
298 case GYRO_EXTI_NO_INT
:
300 gyro
->dev
.txBuf
[0] = gyro
->gyroDataReg
| 0x80;
302 busSegment_t segments
[] = {
303 {.u
.buffers
= {NULL
, NULL
}, 7, true, NULL
},
304 {.u
.link
= {NULL
, NULL
}, 0, true, NULL
},
306 segments
[0].u
.buffers
.txData
= gyro
->dev
.txBuf
;
307 segments
[0].u
.buffers
.rxData
= &gyro
->dev
.rxBuf
[1];
309 spiSequence(&gyro
->dev
, &segments
[0]);
311 // Wait for completion
314 gyro
->gyroADCRaw
[X
] = __builtin_bswap16(gyroData
[1]);
315 gyro
->gyroADCRaw
[Y
] = __builtin_bswap16(gyroData
[2]);
316 gyro
->gyroADCRaw
[Z
] = __builtin_bswap16(gyroData
[3]);
320 case GYRO_EXTI_INT_DMA
:
322 // Acc and gyro data may not be continuous (MPU6xxx has temperature in between)
323 const uint8_t gyroDataIndex
= ((gyro
->gyroDataReg
- gyro
->accDataReg
) >> 1) + 1;
325 // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
327 gyro
->gyroADCRaw
[X
] = __builtin_bswap16(gyroData
[gyroDataIndex
]);
328 gyro
->gyroADCRaw
[Y
] = __builtin_bswap16(gyroData
[gyroDataIndex
+ 1]);
329 gyro
->gyroADCRaw
[Z
] = __builtin_bswap16(gyroData
[gyroDataIndex
+ 2]);
340 typedef uint8_t (*gyroSpiDetectFn_t
)(const extDevice_t
*dev
);
342 static gyroSpiDetectFn_t gyroSpiDetectFnTable
[] = {
343 #ifdef USE_ACCGYRO_LSM6DSV16X
346 #ifdef USE_GYRO_SPI_ICM20689
347 icm20689SpiDetect
, // icm20689SpiDetect detects ICM20602 and ICM20689
349 #ifdef USE_GYRO_SPI_MPU6000
352 #ifdef USE_GYRO_SPI_MPU6500
353 mpu6500SpiDetect
, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
355 #ifdef USE_GYRO_SPI_MPU9250
358 #ifdef USE_ACCGYRO_LSM6DSO
361 #ifdef USE_ACCGYRO_BMI160
364 #ifdef USE_ACCGYRO_BMI270
367 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
370 #ifdef USE_GYRO_SPI_ICM20649
373 #ifdef USE_GYRO_L3GD20
376 NULL
// Avoid an empty array
379 static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t
*gyro
, const gyroDeviceConfig_t
*config
)
381 if (!config
->csnTag
|| !spiSetBusInstance(&gyro
->dev
, config
->spiBus
)) {
385 gyro
->dev
.busType_u
.spi
.csnPin
= IOGetByTag(config
->csnTag
);
387 IOInit(gyro
->dev
.busType_u
.spi
.csnPin
, OWNER_GYRO_CS
, RESOURCE_INDEX(config
->index
));
388 IOConfigGPIO(gyro
->dev
.busType_u
.spi
.csnPin
, SPI_IO_CS_CFG
);
389 IOHi(gyro
->dev
.busType_u
.spi
.csnPin
); // Ensure device is disabled, important when two devices are on the same bus.
391 // Allow 100ms before attempting to access gyro's SPI bus
392 // Do this once here rather than in each detection routine to speed boot
393 while (millis() < GYRO_SPI_STARTUP_MS
);
395 // Set a slow SPI clock that all potential devices can handle during gyro detection
396 spiSetClkDivisor(&gyro
->dev
, spiCalculateDivider(MPU_MAX_SPI_DETECT_CLK_HZ
));
398 // It is hard to use hardware to optimize the detection loop here,
399 // as hardware type and detection function name doesn't match.
400 // May need a bitmap of hardware to detection function to do it right?
402 for (size_t index
= 0 ; gyroSpiDetectFnTable
[index
] ; index
++) {
403 uint8_t sensor
= (gyroSpiDetectFnTable
[index
])(&gyro
->dev
);
404 if (sensor
!= MPU_NONE
) {
405 gyro
->mpuDetectionResult
.sensor
= sensor
;
406 busDeviceRegister(&gyro
->dev
);
411 // Detection failed, disable CS pin again
413 spiPreinitByTag(config
->csnTag
);
419 void mpuPreInit(const struct gyroDeviceConfig_s
*config
)
422 spiPreinitRegister(config
->csnTag
, IOCFG_IPU
, 1);
428 bool mpuDetect(gyroDev_t
*gyro
, const gyroDeviceConfig_t
*config
)
430 static busDevice_t bus
;
431 gyro
->dev
.bus
= &bus
;
433 // MPU datasheet specifies 30ms.
436 if (config
->busType
== BUS_TYPE_NONE
) {
440 if (config
->busType
== BUS_TYPE_GYRO_AUTO
) {
441 gyro
->dev
.bus
->busType
= BUS_TYPE_I2C
;
443 gyro
->dev
.bus
->busType
= config
->busType
;
447 if (gyro
->dev
.bus
->busType
== BUS_TYPE_I2C
) {
448 gyro
->dev
.bus
->busType_u
.i2c
.device
= I2C_CFG_TO_DEV(config
->i2cBus
);
449 gyro
->dev
.busType_u
.i2c
.address
= config
->i2cAddress
? config
->i2cAddress
: MPU_ADDRESS
;
452 bool ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_WHO_AM_I
, &sig
, 1);
455 busDeviceRegister(&gyro
->dev
);
456 // If an MPU3050 is connected sig will contain 0.
457 uint8_t inquiryResult
;
458 ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_WHO_AM_I_LEGACY
, &inquiryResult
, 1);
459 inquiryResult
&= MPU_INQUIRY_MASK
;
460 if (ack
&& inquiryResult
== MPUx0x0_WHO_AM_I_CONST
) {
461 gyro
->mpuDetectionResult
.sensor
= MPU_3050
;
465 sig
&= MPU_INQUIRY_MASK
;
466 if (sig
== MPUx0x0_WHO_AM_I_CONST
) {
467 gyro
->mpuDetectionResult
.sensor
= MPU_60x0
;
468 mpu6050FindRevision(gyro
);
469 } else if (sig
== MPU6500_WHO_AM_I_CONST
) {
470 gyro
->mpuDetectionResult
.sensor
= MPU_65xx_I2C
;
478 gyro
->dev
.bus
->busType
= BUS_TYPE_SPI
;
480 return detectSPISensorsAndUpdateDetectionResult(gyro
, config
);
486 void mpuGyroInit(gyroDev_t
*gyro
)
488 gyro
->accDataReg
= MPU_RA_ACCEL_XOUT_H
;
489 gyro
->gyroDataReg
= MPU_RA_GYRO_XOUT_H
;
490 mpuIntExtiInit(gyro
);
493 uint8_t mpuGyroDLPF(gyroDev_t
*gyro
)
497 // If gyro is in 32KHz mode then the DLPF bits aren't used
498 if (gyro
->gyroRateKHz
<= GYRO_RATE_8_kHz
) {
499 switch (gyro
->hardware_lpf
) {
500 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
501 case GYRO_HARDWARE_LPF_EXPERIMENTAL
:
502 // experimental mode not supported for MPU60x0 family
503 if ((gyro
->gyroHardware
!= GYRO_MPU6050
) && (gyro
->gyroHardware
!= GYRO_MPU6000
)) {
511 case GYRO_HARDWARE_LPF_NORMAL
:
520 #ifdef USE_GYRO_REGISTER_DUMP
521 uint8_t mpuGyroReadRegister(const extDevice_t
*dev
, uint8_t reg
)
524 const bool ack
= busReadRegisterBuffer(dev
, reg
, &data
, 1);