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/>.
23 * Dominic Clifton - Cleanflight implementation
24 * John Ihlein - Initial FF32 code
32 #if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000)
34 #include "common/axis.h"
35 #include "common/maths.h"
37 #include "drivers/accgyro/accgyro.h"
38 #include "drivers/accgyro/accgyro_mpu.h"
39 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
40 #include "drivers/bus_spi.h"
41 #include "drivers/exti.h"
42 #include "drivers/io.h"
43 #include "drivers/time.h"
44 #include "drivers/sensor.h"
45 #include "drivers/system.h"
48 static void mpu6000AccAndGyroInit(gyroDev_t
*gyro
);
50 // 20 MHz max SPI frequency
51 #define MPU6000_MAX_SPI_CLK_HZ 20000000
53 #define MPU6000_SHORT_THRESHOLD 82 // Any interrupt interval less than this will be recognised as the short interval of ~79us
56 #define BIT_SLEEP 0x40
57 #define BIT_H_RESET 0x80
58 #define BITS_CLKSEL 0x07
59 #define MPU_CLK_SEL_PLLGYROX 0x01
60 #define MPU_CLK_SEL_PLLGYROZ 0x03
61 #define MPU_EXT_SYNC_GYROX 0x02
62 #define BITS_FS_250DPS 0x00
63 #define BITS_FS_500DPS 0x08
64 #define BITS_FS_1000DPS 0x10
65 #define BITS_FS_2000DPS 0x18
66 #define BITS_FS_2G 0x00
67 #define BITS_FS_4G 0x08
68 #define BITS_FS_8G 0x10
69 #define BITS_FS_16G 0x18
70 #define BITS_FS_MASK 0x18
71 #define BITS_DLPF_CFG_256HZ 0x00
72 #define BITS_DLPF_CFG_188HZ 0x01
73 #define BITS_DLPF_CFG_98HZ 0x02
74 #define BITS_DLPF_CFG_42HZ 0x03
75 #define BITS_DLPF_CFG_20HZ 0x04
76 #define BITS_DLPF_CFG_10HZ 0x05
77 #define BITS_DLPF_CFG_5HZ 0x06
78 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
79 #define BITS_DLPF_CFG_MASK 0x07
80 #define BIT_INT_ANYRD_2CLEAR 0x10
81 #define BIT_RAW_RDY_EN 0x01
82 #define BIT_I2C_IF_DIS 0x10
83 #define BIT_INT_STATUS_DATA 0x01
88 // Product ID Description for MPU6000
89 // high 4 bits low 4 bits
90 // Product Name Product Revision
91 #define MPU6000ES_REV_C4 0x14
92 #define MPU6000ES_REV_C5 0x15
93 #define MPU6000ES_REV_D6 0x16
94 #define MPU6000ES_REV_D7 0x17
95 #define MPU6000ES_REV_D8 0x18
96 #define MPU6000_REV_C4 0x54
97 #define MPU6000_REV_C5 0x55
98 #define MPU6000_REV_D6 0x56
99 #define MPU6000_REV_D7 0x57
100 #define MPU6000_REV_D8 0x58
101 #define MPU6000_REV_D9 0x59
102 #define MPU6000_REV_D10 0x5A
104 void mpu6000SpiGyroInit(gyroDev_t
*gyro
)
106 extDevice_t
*dev
= &gyro
->dev
;
110 mpu6000AccAndGyroInit(gyro
);
112 // Accel and Gyro DLPF Setting
113 spiWriteReg(dev
, MPU6000_CONFIG
, mpuGyroDLPF(gyro
));
114 delayMicroseconds(1);
116 spiSetClkDivisor(dev
, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ
));
120 if (((int8_t)gyro
->gyroADCRaw
[1]) == -1 && ((int8_t)gyro
->gyroADCRaw
[0]) == -1) {
121 failureMode(FAILURE_GYRO_INIT_FAILED
);
125 void mpu6000SpiAccInit(accDev_t
*acc
)
127 acc
->acc_1G
= 512 * 4;
130 uint8_t mpu6000SpiDetect(const extDevice_t
*dev
)
132 // reset the device configuration
133 spiWriteReg(dev
, MPU_RA_PWR_MGMT_1
, BIT_H_RESET
);
134 delay(100); // datasheet specifies a 100ms delay after reset
136 const uint8_t whoAmI
= spiReadRegMsk(dev
, MPU_RA_WHO_AM_I
);
137 delayMicroseconds(1); // Ensure CS high time is met which is violated on H7 without this delay
138 uint8_t detectedSensor
= MPU_NONE
;
140 if (whoAmI
== MPU6000_WHO_AM_I_CONST
) {
141 const uint8_t productID
= spiReadRegMsk(dev
, MPU_RA_PRODUCT_ID
);
143 /* look for a product ID we recognise */
145 // verify product revision
147 case MPU6000ES_REV_C4
:
148 case MPU6000ES_REV_C5
:
151 case MPU6000ES_REV_D6
:
152 case MPU6000ES_REV_D7
:
153 case MPU6000ES_REV_D8
:
158 case MPU6000_REV_D10
:
159 detectedSensor
= MPU_60x0_SPI
;
162 // reset the device signal paths
163 spiWriteReg(dev
, MPU_RA_SIGNAL_PATH_RESET
, BIT_GYRO
| BIT_ACC
| BIT_TEMP
);
164 delay(100); // datasheet specifies a 100ms delay after signal path reset
167 return detectedSensor
;
170 static void mpu6000AccAndGyroInit(gyroDev_t
*gyro
)
172 extDevice_t
*dev
= &gyro
->dev
;
174 // Device was already reset during detection so proceed with configuration
176 // Clock Source PPL with Z axis gyro reference
177 spiWriteReg(dev
, MPU_RA_PWR_MGMT_1
, MPU_CLK_SEL_PLLGYROZ
);
178 delayMicroseconds(15);
180 // Disable Primary I2C Interface
181 spiWriteReg(dev
, MPU_RA_USER_CTRL
, BIT_I2C_IF_DIS
);
182 delayMicroseconds(15);
184 spiWriteReg(dev
, MPU_RA_PWR_MGMT_2
, 0x00);
185 delayMicroseconds(15);
187 // Accel Sample Rate 1kHz
188 // Gyroscope Output Rate = 1kHz when the DLPF is enabled
189 spiWriteReg(dev
, MPU_RA_SMPLRT_DIV
, gyro
->mpuDividerDrops
);
190 delayMicroseconds(15);
192 // Gyro +/- 2000 DPS Full Scale
193 spiWriteReg(dev
, MPU_RA_GYRO_CONFIG
, INV_FSR_2000DPS
<< 3);
194 delayMicroseconds(15);
196 // Accel +/- 16 G Full Scale
197 spiWriteReg(dev
, MPU_RA_ACCEL_CONFIG
, INV_FSR_16G
<< 3);
198 delayMicroseconds(15);
200 spiWriteReg(dev
, MPU_RA_INT_PIN_CFG
, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
201 delayMicroseconds(15);
203 spiWriteReg(dev
, MPU_RA_INT_ENABLE
, MPU_RF_DATA_RDY_EN
);
204 delayMicroseconds(15);
207 bool mpu6000SpiAccDetect(accDev_t
*acc
)
209 if (acc
->mpuDetectionResult
.sensor
!= MPU_60x0_SPI
) {
213 acc
->initFn
= mpu6000SpiAccInit
;
214 acc
->readFn
= mpuAccReadSPI
;
219 bool mpu6000SpiGyroDetect(gyroDev_t
*gyro
)
221 if (gyro
->mpuDetectionResult
.sensor
!= MPU_60x0_SPI
) {
225 gyro
->initFn
= mpu6000SpiGyroInit
;
226 gyro
->readFn
= mpuGyroReadSPI
;
227 gyro
->scale
= GYRO_SCALE_2000DPS
;
228 gyro
->gyroShortPeriod
= clockMicrosToCycles(MPU6000_SHORT_THRESHOLD
);