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/>.
21 // NOTE: This gyro is considered obsolete and may be removed in the future.
28 #include "common/maths.h"
29 #include "common/utils.h"
31 #include "drivers/bus_i2c.h"
32 #include "drivers/exti.h"
33 #include "drivers/sensor.h"
34 #include "drivers/system.h"
35 #include "drivers/time.h"
38 #include "accgyro_mpu.h"
39 #include "accgyro_mpu3050.h"
41 // MPU3050, Standard address 0x68
42 #define MPU3050_ADDRESS 0x68
45 #define MPU3050_FS_SEL_2000DPS 0x18
46 #define MPU3050_DLPF_10HZ 0x05
47 #define MPU3050_DLPF_20HZ 0x04
48 #define MPU3050_DLPF_42HZ 0x03
49 #define MPU3050_DLPF_98HZ 0x02
50 #define MPU3050_DLPF_188HZ 0x01
51 #define MPU3050_DLPF_256HZ 0x00
53 #define MPU3050_USER_RESET 0x01
54 #define MPU3050_CLK_SEL_PLL_GX 0x01
56 static uint8_t mpu3050GetDLPF(uint8_t lpf
)
59 if (lpf
== GYRO_HARDWARE_LPF_1KHZ_SAMPLE
) {
60 ret
= MPU3050_DLPF_188HZ
;
62 ret
= MPU3050_DLPF_256HZ
;
67 static void mpu3050Init(gyroDev_t
*gyro
)
69 delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
71 const bool ack
= busWriteRegister(&gyro
->bus
, MPU3050_SMPLRT_DIV
, 0);
73 failureMode(FAILURE_ACC_INIT
);
76 busWriteRegister(&gyro
->bus
, MPU3050_DLPF_FS_SYNC
, MPU3050_FS_SEL_2000DPS
| mpu3050GetDLPF(gyro
->hardware_lpf
));
77 busWriteRegister(&gyro
->bus
, MPU3050_INT_CFG
, 0);
78 busWriteRegister(&gyro
->bus
, MPU3050_USER_CTRL
, MPU3050_USER_RESET
);
79 busWriteRegister(&gyro
->bus
, MPU3050_PWR_MGM
, MPU3050_CLK_SEL_PLL_GX
);
82 static bool mpu3050GyroRead(gyroDev_t
*gyro
)
86 const bool ack
= busReadRegisterBuffer(&gyro
->bus
, MPU3050_GYRO_OUT
, data
, 6);
91 gyro
->gyroADCRaw
[X
] = (int16_t)((data
[0] << 8) | data
[1]);
92 gyro
->gyroADCRaw
[Y
] = (int16_t)((data
[2] << 8) | data
[3]);
93 gyro
->gyroADCRaw
[Z
] = (int16_t)((data
[4] << 8) | data
[5]);
98 static bool mpu3050ReadTemperature(gyroDev_t
*gyro
, int16_t *tempData
)
101 if (!busReadRegisterBuffer(&gyro
->bus
, MPU3050_TEMP_OUT
, buf
, 2)) {
105 *tempData
= 35 + ((int32_t)(buf
[0] << 8 | buf
[1]) + 13200) / 280;
110 bool mpu3050Detect(gyroDev_t
*gyro
)
112 if (gyro
->mpuDetectionResult
.sensor
!= MPU_3050
) {
115 gyro
->initFn
= mpu3050Init
;
116 gyro
->readFn
= mpu3050GyroRead
;
117 gyro
->temperatureFn
= mpu3050ReadTemperature
;
119 // 16.4 dps/lsb scalefactor
120 gyro
->scale
= 1.0f
/ 16.4f
;