Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity
betaflight/src/drv_mpu6050.c

269 lines
9.6 KiB
C
Raw Blame History

#include "board.h"
// MPU6050, Standard address 0x68
// MPU_INT on PB13 on rev4 hardware
#define MPU6050_ADDRESS 0x68
#define DMP_MEM_START_ADDR 0x6E
#define DMP_MEM_R_W 0x6F
#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
#define MPU_RA_XA_OFFS_L_TC 0x07
#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
#define MPU_RA_YA_OFFS_L_TC 0x09
#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
#define MPU_RA_ZA_OFFS_L_TC 0x0B
#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
#define MPU_RA_XG_OFFS_USRL 0x14
#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
#define MPU_RA_YG_OFFS_USRL 0x16
#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
#define MPU_RA_ZG_OFFS_USRL 0x18
#define MPU_RA_SMPLRT_DIV 0x19
#define MPU_RA_CONFIG 0x1A
#define MPU_RA_GYRO_CONFIG 0x1B
#define MPU_RA_ACCEL_CONFIG 0x1C
#define MPU_RA_FF_THR 0x1D
#define MPU_RA_FF_DUR 0x1E
#define MPU_RA_MOT_THR 0x1F
#define MPU_RA_MOT_DUR 0x20
#define MPU_RA_ZRMOT_THR 0x21
#define MPU_RA_ZRMOT_DUR 0x22
#define MPU_RA_FIFO_EN 0x23
#define MPU_RA_I2C_MST_CTRL 0x24
#define MPU_RA_I2C_SLV0_ADDR 0x25
#define MPU_RA_I2C_SLV0_REG 0x26
#define MPU_RA_I2C_SLV0_CTRL 0x27
#define MPU_RA_I2C_SLV1_ADDR 0x28
#define MPU_RA_I2C_SLV1_REG 0x29
#define MPU_RA_I2C_SLV1_CTRL 0x2A
#define MPU_RA_I2C_SLV2_ADDR 0x2B
#define MPU_RA_I2C_SLV2_REG 0x2C
#define MPU_RA_I2C_SLV2_CTRL 0x2D
#define MPU_RA_I2C_SLV3_ADDR 0x2E
#define MPU_RA_I2C_SLV3_REG 0x2F
#define MPU_RA_I2C_SLV3_CTRL 0x30
#define MPU_RA_I2C_SLV4_ADDR 0x31
#define MPU_RA_I2C_SLV4_REG 0x32
#define MPU_RA_I2C_SLV4_DO 0x33
#define MPU_RA_I2C_SLV4_CTRL 0x34
#define MPU_RA_I2C_SLV4_DI 0x35
#define MPU_RA_I2C_MST_STATUS 0x36
#define MPU_RA_INT_PIN_CFG 0x37
#define MPU_RA_INT_ENABLE 0x38
#define MPU_RA_DMP_INT_STATUS 0x39
#define MPU_RA_INT_STATUS 0x3A
#define MPU_RA_ACCEL_XOUT_H 0x3B
#define MPU_RA_ACCEL_XOUT_L 0x3C
#define MPU_RA_ACCEL_YOUT_H 0x3D
#define MPU_RA_ACCEL_YOUT_L 0x3E
#define MPU_RA_ACCEL_ZOUT_H 0x3F
#define MPU_RA_ACCEL_ZOUT_L 0x40
#define MPU_RA_TEMP_OUT_H 0x41
#define MPU_RA_TEMP_OUT_L 0x42
#define MPU_RA_GYRO_XOUT_H 0x43
#define MPU_RA_GYRO_XOUT_L 0x44
#define MPU_RA_GYRO_YOUT_H 0x45
#define MPU_RA_GYRO_YOUT_L 0x46
#define MPU_RA_GYRO_ZOUT_H 0x47
#define MPU_RA_GYRO_ZOUT_L 0x48
#define MPU_RA_EXT_SENS_DATA_00 0x49
#define MPU_RA_MOT_DETECT_STATUS 0x61
#define MPU_RA_I2C_SLV0_DO 0x63
#define MPU_RA_I2C_SLV1_DO 0x64
#define MPU_RA_I2C_SLV2_DO 0x65
#define MPU_RA_I2C_SLV3_DO 0x66
#define MPU_RA_I2C_MST_DELAY_CTRL 0x67
#define MPU_RA_SIGNAL_PATH_RESET 0x68
#define MPU_RA_MOT_DETECT_CTRL 0x69
#define MPU_RA_USER_CTRL 0x6A
#define MPU_RA_PWR_MGMT_1 0x6B
#define MPU_RA_PWR_MGMT_2 0x6C
#define MPU_RA_BANK_SEL 0x6D
#define MPU_RA_MEM_START_ADDR 0x6E
#define MPU_RA_MEM_R_W 0x6F
#define MPU_RA_DMP_CFG_1 0x70
#define MPU_RA_DMP_CFG_2 0x71
#define MPU_RA_FIFO_COUNTH 0x72
#define MPU_RA_FIFO_COUNTL 0x73
#define MPU_RA_FIFO_R_W 0x74
#define MPU_RA_WHO_AM_I 0x75
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
#define MPU6050_LPF_256HZ 0
#define MPU6050_LPF_188HZ 1
#define MPU6050_LPF_98HZ 2
#define MPU6050_LPF_42HZ 3
#define MPU6050_LPF_20HZ 4
#define MPU6050_LPF_10HZ 5
#define MPU6050_LPF_5HZ 6
static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
static sensor_align_e gyroAlign = CW0_DEG;
static sensor_align_e accAlign = CW0_DEG;
static void mpu6050AccInit(sensor_align_e align);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(sensor_align_e align);
static void mpu6050GyroRead(int16_t *gyroData);
extern uint16_t acc_1G;
static uint8_t mpuAccelHalf = 0;
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale)
{
bool ack;
uint8_t sig, rev;
uint8_t tmp[6];
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
if (!ack)
return false;
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
// The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0<58>s 7-bit I2C address.
// The least significant bit of the MPU-60X0<58>s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig != (MPU6050_ADDRESS & 0x7e))
return false;
// determine product ID and accel revision
i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, tmp);
rev = ((tmp[5] & 0x01) << 2) | ((tmp[3] & 0x01) << 1) | (tmp[1] & 0x01);
if (rev) {
/* Congrats, these parts are better. */
if (rev == 1) {
mpuAccelHalf = 1;
} else if (rev == 2) {
mpuAccelHalf = 0;
} else {
failureMode(5);
}
} else {
i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &sig);
rev = sig & 0x0F;
if (!rev) {
failureMode(5);
} else if (rev == 4) {
mpuAccelHalf = 1;
} else {
mpuAccelHalf = 0;
}
}
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
// give halfacc (old revision) back to system
if (scale)
*scale = mpuAccelHalf;
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU6050_LPF_256HZ;
break;
case 188:
mpuLowPassFilter = MPU6050_LPF_188HZ;
break;
case 98:
mpuLowPassFilter = MPU6050_LPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU6050_LPF_42HZ;
break;
case 20:
mpuLowPassFilter = MPU6050_LPF_20HZ;
break;
case 10:
mpuLowPassFilter = MPU6050_LPF_10HZ;
break;
case 5:
mpuLowPassFilter = MPU6050_LPF_5HZ;
break;
}
return true;
}
static void mpu6050AccInit(sensor_align_e align)
{
if (mpuAccelHalf)
acc_1G = 255 * 8;
else
acc_1G = 512 * 8;
if (align > 0)
accAlign = align;
}
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
alignSensors(data, accData, accAlign);
}
static void mpu6050GyroInit(sensor_align_e align)
{
gpio_config_t gpio;
// MPU_INT output on rev4/5 hardware (PB13, PC13)
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
if (hse_value == 8000000)
gpioInit(GPIOB, &gpio);
else if (hse_value == 12000000)
gpioInit(GPIOC, &gpio);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
// Accel scale 8g (4096 LSB/g)
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
if (align > 0)
gyroAlign = align;
}
static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
Reference in a new issue View git blame Copy permalink