2 * This file is part of Cleanflight.
4 * Cleanflight 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.
9 * Cleanflight 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
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
21 * Dominic Clifton - Cleanflight implementation
22 * John Ihlein - Initial FF32 code
30 #include "common/axis.h"
31 #include "common/maths.h"
38 #include "gyro_sync.h"
42 #include "accgyro_mpu.h"
44 #if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000)
46 #include "accgyro_spi_mpu6000.h"
48 static void mpu6000AccAndGyroInit(void);
50 static bool mpuSpi6000InitDone
= false;
54 #define BIT_SLEEP 0x40
55 #define BIT_H_RESET 0x80
56 #define BITS_CLKSEL 0x07
57 #define MPU_CLK_SEL_PLLGYROX 0x01
58 #define MPU_CLK_SEL_PLLGYROZ 0x03
59 #define MPU_EXT_SYNC_GYROX 0x02
60 #define BITS_FS_250DPS 0x00
61 #define BITS_FS_500DPS 0x08
62 #define BITS_FS_1000DPS 0x10
63 #define BITS_FS_2000DPS 0x18
64 #define BITS_FS_2G 0x00
65 #define BITS_FS_4G 0x08
66 #define BITS_FS_8G 0x10
67 #define BITS_FS_16G 0x18
68 #define BITS_FS_MASK 0x18
69 #define BITS_DLPF_CFG_256HZ 0x00
70 #define BITS_DLPF_CFG_188HZ 0x01
71 #define BITS_DLPF_CFG_98HZ 0x02
72 #define BITS_DLPF_CFG_42HZ 0x03
73 #define BITS_DLPF_CFG_20HZ 0x04
74 #define BITS_DLPF_CFG_10HZ 0x05
75 #define BITS_DLPF_CFG_5HZ 0x06
76 #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
77 #define BITS_DLPF_CFG_MASK 0x07
78 #define BIT_INT_ANYRD_2CLEAR 0x10
79 #define BIT_RAW_RDY_EN 0x01
80 #define BIT_I2C_IF_DIS 0x10
81 #define BIT_INT_STATUS_DATA 0x01
86 // Product ID Description for MPU6000
87 // high 4 bits low 4 bits
88 // Product Name Product Revision
89 #define MPU6000ES_REV_C4 0x14
90 #define MPU6000ES_REV_C5 0x15
91 #define MPU6000ES_REV_D6 0x16
92 #define MPU6000ES_REV_D7 0x17
93 #define MPU6000ES_REV_D8 0x18
94 #define MPU6000_REV_C4 0x54
95 #define MPU6000_REV_C5 0x55
96 #define MPU6000_REV_D6 0x56
97 #define MPU6000_REV_D7 0x57
98 #define MPU6000_REV_D8 0x58
99 #define MPU6000_REV_D9 0x59
100 #define MPU6000_REV_D10 0x5A
102 #define DISABLE_MPU6000 IOHi(mpuSpi6000CsPin)
103 #define ENABLE_MPU6000 IOLo(mpuSpi6000CsPin)
105 static IO_t mpuSpi6000CsPin
= IO_NONE
;
107 bool mpu6000WriteRegister(uint8_t reg
, uint8_t data
)
110 spiTransferByte(MPU6000_SPI_INSTANCE
, reg
);
111 spiTransferByte(MPU6000_SPI_INSTANCE
, data
);
117 bool mpu6000ReadRegister(uint8_t reg
, uint8_t length
, uint8_t *data
)
120 spiTransferByte(MPU6000_SPI_INSTANCE
, reg
| 0x80); // read transaction
121 spiTransfer(MPU6000_SPI_INSTANCE
, data
, NULL
, length
);
127 void mpu6000SpiGyroInit(uint8_t lpf
)
131 mpu6000AccAndGyroInit();
133 spiSetDivisor(MPU6000_SPI_INSTANCE
, SPI_CLOCK_INITIALIZATON
);
135 // Accel and Gyro DLPF Setting
136 mpu6000WriteRegister(MPU6000_CONFIG
, lpf
);
137 delayMicroseconds(1);
139 spiSetDivisor(MPU6000_SPI_INSTANCE
, SPI_CLOCK_FAST
); // 18 MHz SPI clock
144 if (((int8_t)data
[1]) == -1 && ((int8_t)data
[0]) == -1) {
145 failureMode(FAILURE_GYRO_INIT_FAILED
);
149 void mpu6000SpiAccInit(acc_t
*acc
)
153 acc
->acc_1G
= 512 * 4;
156 bool mpu6000SpiDetect(void)
159 uint8_t attemptsRemaining
= 5;
161 #ifdef MPU6000_CS_PIN
162 mpuSpi6000CsPin
= IOGetByTag(IO_TAG(MPU6000_CS_PIN
));
164 IOInit(mpuSpi6000CsPin
, OWNER_MPU
, RESOURCE_SPI_CS
, 0);
165 IOConfigGPIO(mpuSpi6000CsPin
, SPI_IO_CS_CFG
);
167 spiSetDivisor(MPU6000_SPI_INSTANCE
, SPI_CLOCK_INITIALIZATON
);
169 mpu6000WriteRegister(MPU_RA_PWR_MGMT_1
, BIT_H_RESET
);
174 mpu6000ReadRegister(MPU_RA_WHO_AM_I
, 1, &in
);
175 if (in
== MPU6000_WHO_AM_I_CONST
) {
178 if (!attemptsRemaining
) {
181 } while (attemptsRemaining
--);
184 mpu6000ReadRegister(MPU_RA_PRODUCT_ID
, 1, &in
);
186 /* look for a product ID we recognise */
188 // verify product revision
190 case MPU6000ES_REV_C4
:
191 case MPU6000ES_REV_C5
:
194 case MPU6000ES_REV_D6
:
195 case MPU6000ES_REV_D7
:
196 case MPU6000ES_REV_D8
:
201 case MPU6000_REV_D10
:
208 static void mpu6000AccAndGyroInit(void)
210 if (mpuSpi6000InitDone
) {
214 spiSetDivisor(MPU6000_SPI_INSTANCE
, SPI_CLOCK_INITIALIZATON
);
217 mpu6000WriteRegister(MPU_RA_PWR_MGMT_1
, BIT_H_RESET
);
220 mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET
, BIT_GYRO
| BIT_ACC
| BIT_TEMP
);
223 // Clock Source PPL with Z axis gyro reference
224 mpu6000WriteRegister(MPU_RA_PWR_MGMT_1
, MPU_CLK_SEL_PLLGYROZ
);
225 delayMicroseconds(15);
227 // Disable Primary I2C Interface
228 mpu6000WriteRegister(MPU_RA_USER_CTRL
, BIT_I2C_IF_DIS
);
229 delayMicroseconds(15);
231 mpu6000WriteRegister(MPU_RA_PWR_MGMT_2
, 0x00);
232 delayMicroseconds(15);
234 // Accel Sample Rate 1kHz
235 // Gyroscope Output Rate = 1kHz when the DLPF is enabled
236 mpu6000WriteRegister(MPU_RA_SMPLRT_DIV
, gyroMPU6xxxGetDividerDrops());
237 delayMicroseconds(15);
239 // Gyro +/- 1000 DPS Full Scale
240 mpu6000WriteRegister(MPU_RA_GYRO_CONFIG
, INV_FSR_2000DPS
<< 3);
241 delayMicroseconds(15);
243 // Accel +/- 8 G Full Scale
244 mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG
, INV_FSR_16G
<< 3);
245 delayMicroseconds(15);
248 mpu6000WriteRegister(MPU_RA_INT_PIN_CFG
, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
249 delayMicroseconds(15);
251 #ifdef USE_MPU_DATA_READY_SIGNAL
252 mpu6000WriteRegister(MPU_RA_INT_ENABLE
, MPU_RF_DATA_RDY_EN
);
253 delayMicroseconds(15);
256 spiSetDivisor(MPU6000_SPI_INSTANCE
, SPI_CLOCK_FAST
);
257 delayMicroseconds(1);
259 mpuSpi6000InitDone
= true;
262 bool mpu6000SpiAccDetect(acc_t
*acc
)
264 if (mpuDetectionResult
.sensor
!= MPU_60x0_SPI
) {
268 acc
->init
= mpu6000SpiAccInit
;
269 acc
->read
= mpuAccRead
;
274 bool mpu6000SpiGyroDetect(gyro_t
*gyro
)
276 if (mpuDetectionResult
.sensor
!= MPU_60x0_SPI
) {
280 gyro
->init
= mpu6000SpiGyroInit
;
281 gyro
->read
= mpuGyroRead
;
282 gyro
->intStatus
= checkMPUDataReady
;
283 // 16.4 dps/lsb scalefactor
284 gyro
->scale
= 1.0f
/ 16.4f
;