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_mpu.h"
62 #include "pg/gyrodev.h"
65 #define MPU_ADDRESS 0x68
68 // 1 MHz max SPI frequency during device detection
69 #define MPU_MAX_SPI_DETECT_CLK_HZ 1000000
71 #define MPU_INQUIRY_MASK 0x7E
73 // Allow 100ms before attempting to access SPI bus
74 #define GYRO_SPI_STARTUP_MS 100
76 // Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
77 #define GYRO_EXTI_DETECT_THRESHOLD 1000
80 static void mpu6050FindRevision(gyroDev_t
*gyro
)
82 // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
83 // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
85 // determine product ID and revision
86 uint8_t readBuffer
[6];
87 bool ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_XA_OFFS_H
, readBuffer
, 6);
88 uint8_t revision
= ((readBuffer
[5] & 0x01) << 2) | ((readBuffer
[3] & 0x01) << 1) | (readBuffer
[1] & 0x01);
89 if (ack
&& revision
) {
90 // Congrats, these parts are better
92 gyro
->mpuDetectionResult
.resolution
= MPU_HALF_RESOLUTION
;
93 } else if (revision
== 2) {
94 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
95 } else if ((revision
== 3) || (revision
== 7)) {
96 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
98 failureMode(FAILURE_ACC_INCOMPATIBLE
);
102 ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_PRODUCT_ID
, &productId
, 1);
103 revision
= productId
& 0x0F;
104 if (!ack
|| revision
== 0) {
105 failureMode(FAILURE_ACC_INCOMPATIBLE
);
106 } else if (revision
== 4) {
107 gyro
->mpuDetectionResult
.resolution
= MPU_HALF_RESOLUTION
;
109 gyro
->mpuDetectionResult
.resolution
= MPU_FULL_RESOLUTION
;
116 * Gyro interrupt service routine
119 // Called in ISR context
120 // Gyro read has just completed
121 busStatus_e
mpuIntcallback(uint32_t arg
)
123 gyroDev_t
*gyro
= (gyroDev_t
*)arg
;
124 int32_t gyroDmaDuration
= cmpTimeCycles(getCycleCounter(), gyro
->gyroLastEXTI
);
126 if (gyroDmaDuration
> gyro
->gyroDmaMaxDuration
) {
127 gyro
->gyroDmaMaxDuration
= gyroDmaDuration
;
130 gyro
->dataReady
= true;
135 static void mpuIntExtiHandler(extiCallbackRec_t
*cb
)
137 gyroDev_t
*gyro
= container_of(cb
, gyroDev_t
, exti
);
139 // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
140 // not have an associated timer
141 uint32_t nowCycles
= getCycleCounter();
142 int32_t gyroLastPeriod
= cmpTimeCycles(nowCycles
, gyro
->gyroLastEXTI
);
143 // This detects the short (~79us) EXTI interval of an MPU6xxx gyro
144 if ((gyro
->gyroShortPeriod
== 0) || (gyroLastPeriod
< gyro
->gyroShortPeriod
)) {
145 gyro
->gyroSyncEXTI
= gyro
->gyroLastEXTI
+ gyro
->gyroDmaMaxDuration
;
147 gyro
->gyroLastEXTI
= nowCycles
;
149 if (gyro
->gyroModeSPI
== GYRO_EXTI_INT_DMA
) {
150 spiSequence(&gyro
->dev
, gyro
->segments
);
153 gyro
->detectedEXTI
++;
156 static void mpuIntExtiHandler(extiCallbackRec_t
*cb
)
158 gyroDev_t
*gyro
= container_of(cb
, gyroDev_t
, exti
);
159 gyro
->dataReady
= true;
163 static void mpuIntExtiInit(gyroDev_t
*gyro
)
165 if (gyro
->mpuIntExtiTag
== IO_TAG_NONE
) {
169 const IO_t mpuIntIO
= IOGetByTag(gyro
->mpuIntExtiTag
);
171 #ifdef ENSURE_MPU_DATA_READY_IS_LOW
172 uint8_t status
= IORead(mpuIntIO
);
178 IOInit(mpuIntIO
, OWNER_GYRO_EXTI
, 0);
179 EXTIHandlerInit(&gyro
->exti
, mpuIntExtiHandler
);
180 EXTIConfig(mpuIntIO
, &gyro
->exti
, NVIC_PRIO_MPU_INT_EXTI
, IOCFG_IN_FLOATING
, BETAFLIGHT_EXTI_TRIGGER_RISING
);
181 EXTIEnable(mpuIntIO
);
184 bool mpuAccRead(accDev_t
*acc
)
188 const bool ack
= busReadRegisterBuffer(&acc
->gyro
->dev
, acc
->gyro
->accDataReg
, data
, 6);
193 acc
->ADCRaw
[X
] = (int16_t)((data
[0] << 8) | data
[1]);
194 acc
->ADCRaw
[Y
] = (int16_t)((data
[2] << 8) | data
[3]);
195 acc
->ADCRaw
[Z
] = (int16_t)((data
[4] << 8) | data
[5]);
200 bool mpuGyroRead(gyroDev_t
*gyro
)
204 const bool ack
= busReadRegisterBuffer(&gyro
->dev
, gyro
->gyroDataReg
, data
, 6);
209 gyro
->gyroADCRaw
[X
] = (int16_t)((data
[0] << 8) | data
[1]);
210 gyro
->gyroADCRaw
[Y
] = (int16_t)((data
[2] << 8) | data
[3]);
211 gyro
->gyroADCRaw
[Z
] = (int16_t)((data
[4] << 8) | data
[5]);
218 bool mpuAccReadSPI(accDev_t
*acc
)
220 switch (acc
->gyro
->gyroModeSPI
) {
222 case GYRO_EXTI_NO_INT
:
224 acc
->gyro
->dev
.txBuf
[0] = acc
->gyro
->accDataReg
| 0x80;
226 busSegment_t segments
[] = {
227 {.u
.buffers
= {NULL
, NULL
}, 7, true, NULL
},
228 {.u
.link
= {NULL
, NULL
}, 0, true, NULL
},
230 segments
[0].u
.buffers
.txData
= acc
->gyro
->dev
.txBuf
;
231 segments
[0].u
.buffers
.rxData
= &acc
->gyro
->dev
.rxBuf
[1];
233 spiSequence(&acc
->gyro
->dev
, &segments
[0]);
235 // Wait for completion
236 spiWait(&acc
->gyro
->dev
);
242 case GYRO_EXTI_INT_DMA
:
244 // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
247 // This data was read from the gyro, which is the same SPI device as the acc
248 int16_t *accData
= (int16_t *)acc
->gyro
->dev
.rxBuf
;
249 acc
->ADCRaw
[X
] = __builtin_bswap16(accData
[1]);
250 acc
->ADCRaw
[Y
] = __builtin_bswap16(accData
[2]);
251 acc
->ADCRaw
[Z
] = __builtin_bswap16(accData
[3]);
263 bool mpuGyroReadSPI(gyroDev_t
*gyro
)
265 int16_t *gyroData
= (int16_t *)gyro
->dev
.rxBuf
;
266 switch (gyro
->gyroModeSPI
) {
269 // Initialise the tx buffer to all 0xff
270 memset(gyro
->dev
.txBuf
, 0xff, 16);
272 // Check that minimum number of interrupts have been detected
274 // We need some offset from the gyro interrupts to ensure sampling after the interrupt
275 gyro
->gyroDmaMaxDuration
= 5;
276 if (gyro
->detectedEXTI
> GYRO_EXTI_DETECT_THRESHOLD
) {
277 if (spiUseDMA(&gyro
->dev
)) {
278 gyro
->dev
.callbackArg
= (uint32_t)gyro
;
279 gyro
->dev
.txBuf
[0] = gyro
->accDataReg
| 0x80;
280 gyro
->segments
[0].len
= gyro
->gyroDataReg
- gyro
->accDataReg
+ 7;
281 gyro
->segments
[0].callback
= mpuIntcallback
;
282 gyro
->segments
[0].u
.buffers
.txData
= gyro
->dev
.txBuf
;
283 gyro
->segments
[0].u
.buffers
.rxData
= &gyro
->dev
.rxBuf
[1];
284 gyro
->segments
[0].negateCS
= true;
285 gyro
->gyroModeSPI
= GYRO_EXTI_INT_DMA
;
287 // Interrupts are present, but no DMA
288 gyro
->gyroModeSPI
= GYRO_EXTI_INT
;
291 gyro
->gyroModeSPI
= GYRO_EXTI_NO_INT
;
297 case GYRO_EXTI_NO_INT
:
299 gyro
->dev
.txBuf
[0] = gyro
->gyroDataReg
| 0x80;
301 busSegment_t segments
[] = {
302 {.u
.buffers
= {NULL
, NULL
}, 7, true, NULL
},
303 {.u
.link
= {NULL
, NULL
}, 0, true, NULL
},
305 segments
[0].u
.buffers
.txData
= gyro
->dev
.txBuf
;
306 segments
[0].u
.buffers
.rxData
= &gyro
->dev
.rxBuf
[1];
308 spiSequence(&gyro
->dev
, &segments
[0]);
310 // Wait for completion
313 gyro
->gyroADCRaw
[X
] = __builtin_bswap16(gyroData
[1]);
314 gyro
->gyroADCRaw
[Y
] = __builtin_bswap16(gyroData
[2]);
315 gyro
->gyroADCRaw
[Z
] = __builtin_bswap16(gyroData
[3]);
319 case GYRO_EXTI_INT_DMA
:
321 // Acc and gyro data may not be continuous (MPU6xxx has temperature in between)
322 const uint8_t gyroDataIndex
= ((gyro
->gyroDataReg
- gyro
->accDataReg
) >> 1) + 1;
324 // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
326 gyro
->gyroADCRaw
[X
] = __builtin_bswap16(gyroData
[gyroDataIndex
]);
327 gyro
->gyroADCRaw
[Y
] = __builtin_bswap16(gyroData
[gyroDataIndex
+ 1]);
328 gyro
->gyroADCRaw
[Z
] = __builtin_bswap16(gyroData
[gyroDataIndex
+ 2]);
339 typedef uint8_t (*gyroSpiDetectFn_t
)(const extDevice_t
*dev
);
341 static gyroSpiDetectFn_t gyroSpiDetectFnTable
[] = {
342 #ifdef USE_GYRO_SPI_MPU6000
345 #ifdef USE_GYRO_SPI_MPU6500
346 mpu6500SpiDetect
, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
348 #ifdef USE_GYRO_SPI_MPU9250
351 #ifdef USE_GYRO_SPI_ICM20689
352 icm20689SpiDetect
, // icm20689SpiDetect detects ICM20602 and ICM20689
354 #ifdef USE_ACCGYRO_LSM6DSO
357 #ifdef USE_ACCGYRO_BMI160
360 #ifdef USE_ACCGYRO_BMI270
363 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
366 #ifdef USE_GYRO_SPI_ICM20649
369 #ifdef USE_GYRO_L3GD20
372 NULL
// Avoid an empty array
375 static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t
*gyro
, const gyroDeviceConfig_t
*config
)
377 if (!config
->csnTag
|| !spiSetBusInstance(&gyro
->dev
, config
->spiBus
)) {
381 gyro
->dev
.busType_u
.spi
.csnPin
= IOGetByTag(config
->csnTag
);
383 IOInit(gyro
->dev
.busType_u
.spi
.csnPin
, OWNER_GYRO_CS
, RESOURCE_INDEX(config
->index
));
384 IOConfigGPIO(gyro
->dev
.busType_u
.spi
.csnPin
, SPI_IO_CS_CFG
);
385 IOHi(gyro
->dev
.busType_u
.spi
.csnPin
); // Ensure device is disabled, important when two devices are on the same bus.
387 uint8_t sensor
= MPU_NONE
;
389 // Allow 100ms before attempting to access gyro's SPI bus
390 // Do this once here rather than in each detection routine to speed boot
391 while (millis() < GYRO_SPI_STARTUP_MS
);
393 // Set a slow SPI clock that all potential devices can handle during gyro detection
394 spiSetClkDivisor(&gyro
->dev
, spiCalculateDivider(MPU_MAX_SPI_DETECT_CLK_HZ
));
396 // It is hard to use hardware to optimize the detection loop here,
397 // as hardware type and detection function name doesn't match.
398 // May need a bitmap of hardware to detection function to do it right?
400 for (size_t index
= 0 ; gyroSpiDetectFnTable
[index
] ; index
++) {
401 sensor
= (gyroSpiDetectFnTable
[index
])(&gyro
->dev
);
402 if (sensor
!= MPU_NONE
) {
403 gyro
->mpuDetectionResult
.sensor
= sensor
;
404 busDeviceRegister(&gyro
->dev
);
409 // Detection failed, disable CS pin again
411 spiPreinitByTag(config
->csnTag
);
417 void mpuPreInit(const struct gyroDeviceConfig_s
*config
)
420 spiPreinitRegister(config
->csnTag
, IOCFG_IPU
, 1);
426 bool mpuDetect(gyroDev_t
*gyro
, const gyroDeviceConfig_t
*config
)
428 static busDevice_t bus
;
429 gyro
->dev
.bus
= &bus
;
431 // MPU datasheet specifies 30ms.
434 if (config
->busType
== BUS_TYPE_NONE
) {
438 if (config
->busType
== BUS_TYPE_GYRO_AUTO
) {
439 gyro
->dev
.bus
->busType
= BUS_TYPE_I2C
;
441 gyro
->dev
.bus
->busType
= config
->busType
;
445 if (gyro
->dev
.bus
->busType
== BUS_TYPE_I2C
) {
446 gyro
->dev
.bus
->busType_u
.i2c
.device
= I2C_CFG_TO_DEV(config
->i2cBus
);
447 gyro
->dev
.busType_u
.i2c
.address
= config
->i2cAddress
? config
->i2cAddress
: MPU_ADDRESS
;
450 bool ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_WHO_AM_I
, &sig
, 1);
453 busDeviceRegister(&gyro
->dev
);
454 // If an MPU3050 is connected sig will contain 0.
455 uint8_t inquiryResult
;
456 ack
= busReadRegisterBuffer(&gyro
->dev
, MPU_RA_WHO_AM_I_LEGACY
, &inquiryResult
, 1);
457 inquiryResult
&= MPU_INQUIRY_MASK
;
458 if (ack
&& inquiryResult
== MPUx0x0_WHO_AM_I_CONST
) {
459 gyro
->mpuDetectionResult
.sensor
= MPU_3050
;
463 sig
&= MPU_INQUIRY_MASK
;
464 if (sig
== MPUx0x0_WHO_AM_I_CONST
) {
465 gyro
->mpuDetectionResult
.sensor
= MPU_60x0
;
466 mpu6050FindRevision(gyro
);
467 } else if (sig
== MPU6500_WHO_AM_I_CONST
) {
468 gyro
->mpuDetectionResult
.sensor
= MPU_65xx_I2C
;
476 gyro
->dev
.bus
->busType
= BUS_TYPE_SPI
;
478 return detectSPISensorsAndUpdateDetectionResult(gyro
, config
);
484 void mpuGyroInit(gyroDev_t
*gyro
)
486 gyro
->accDataReg
= MPU_RA_ACCEL_XOUT_H
;
487 gyro
->gyroDataReg
= MPU_RA_GYRO_XOUT_H
;
488 mpuIntExtiInit(gyro
);
491 uint8_t mpuGyroDLPF(gyroDev_t
*gyro
)
495 // If gyro is in 32KHz mode then the DLPF bits aren't used
496 if (gyro
->gyroRateKHz
<= GYRO_RATE_8_kHz
) {
497 switch (gyro
->hardware_lpf
) {
498 #ifdef USE_GYRO_DLPF_EXPERIMENTAL
499 case GYRO_HARDWARE_LPF_EXPERIMENTAL
:
500 // experimental mode not supported for MPU60x0 family
501 if ((gyro
->gyroHardware
!= GYRO_MPU6050
) && (gyro
->gyroHardware
!= GYRO_MPU6000
)) {
509 case GYRO_HARDWARE_LPF_NORMAL
:
518 #ifdef USE_GYRO_REGISTER_DUMP
519 uint8_t mpuGyroReadRegister(const extDevice_t
*dev
, uint8_t reg
)
522 const bool ack
= busReadRegisterBuffer(dev
, reg
, &data
, 1);