rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added 4K DPS awareness for ICM-20601

This commit is contained in:
Andrey Mironov 2018-07-29 10:13:53 +03:00
parent 74d883bfa8
commit c2243c7fd8
3 changed files with 30 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -156,6 +156,14 @@ enum gyro_fsr_e {
NUM_GYRO_FSR NUM_GYRO_FSR
}; };
enum icm_high_range_gyro_fsr_e {
ICM_HIGH_RANGE_FSR_500DPS = 0,
ICM_HIGH_RANGE_FSR_1000DPS,
ICM_HIGH_RANGE_FSR_2000DPS,
ICM_HIGH_RANGE_FSR_4000DPS,
NUM_ICM_HIGH_RANGE_GYRO_FSR
};
enum fchoice_b { enum fchoice_b {
FCB_DISABLED = 0x00, FCB_DISABLED = 0x00,
FCB_8800_32 = 0x01, FCB_8800_32 = 0x01,
@ -176,6 +184,14 @@ enum accel_fsr_e {
NUM_ACCEL_FSR NUM_ACCEL_FSR
}; };
enum icm_high_range_accel_fsr_e {
ICM_HIGH_RANGE_FSR_4G = 0,
ICM_HIGH_RANGE_FSR_8G,
ICM_HIGH_RANGE_FSR_16G,
ICM_HIGH_RANGE_FSR_32G,
NUM_ICM_HIGH_RANGE_ACCEL_FSR
};
typedef enum { typedef enum {
GYRO_OVERFLOW_NONE = 0x00, GYRO_OVERFLOW_NONE = 0x00,
GYRO_OVERFLOW_X = 0x01, GYRO_OVERFLOW_X = 0x01,

12
src/main/drivers/accgyro/accgyro_mpu6500.c
View File

@ -56,6 +56,14 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
int gyro_range = INV_FSR_2000DPS;
int accel_range = INV_FSR_16G;
if (gyro->mpuDetectionResult.sensor == ICM_20601_SPI) {
gyro_range = gyro->gyro_high_fsr ? ICM_HIGH_RANGE_FSR_4000DPS : ICM_HIGH_RANGE_FSR_2000DPS;
accel_range = ICM_HIGH_RANGE_FSR_16G;
}
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100); delay(100);
busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07); busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
@ -64,9 +72,9 @@ void mpu6500GyroInit(gyroDev_t *gyro)
delay(100); delay(100);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL); busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro)); busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3 | mpuGyroFCHOICE(gyro));
delay(15); delay(15);
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3);
delay(15); delay(15);
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
delay(15); delay(15);

7
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View File

@ -128,7 +128,11 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
case MPU_9250_SPI: case MPU_9250_SPI:
case ICM_20608_SPI: case ICM_20608_SPI:
case ICM_20602_SPI: case ICM_20602_SPI:
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
break;
case ICM_20601_SPI: case ICM_20601_SPI:
gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f);
break; break;
default: default:
return false; return false;
@ -137,8 +141,5 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
gyro->initFn = mpu6500SpiGyroInit; gyro->initFn = mpu6500SpiGyroInit;
gyro->readFn = mpuGyroReadSPI; gyro->readFn = mpuGyroReadSPI;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true; return true;
} }