Skip to content

Commit

Permalink
Fix ICM20469 (#13616)
Browse files Browse the repository at this point in the history 
* Fix ICM20469

* Reorder defines for acc & gyro
  • Loading branch information
@haslinghuis
haslinghuis committed May 10, 2024
1 parent 5a28ce5 commit 33ead2d
Show file tree
Hide file tree
Showing 3 changed files with 34 additions and 30 deletions.
34 changes: 17 additions & 17 deletions src/main/drivers/accgyro/accgyro_spi_icm20649.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ uint8_t icm20649SpiDetect(const extDevice_t *dev)
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t whoAmI = spiReadRegMsk(bus, ICM20649_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(dev, ICM20649_RA_WHO_AM_I);
if (whoAmI == ICM20649_WHO_AM_I_CONST) {
icmDetected = ICM_20649_SPI;
} else {
Expand All @@ -77,12 +77,12 @@ void icm20649AccInit(accDev_t *acc)
// 1,024 LSB/g 30g
acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;

spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiWriteReg(&acc->gyro->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
spiWriteReg(&acc->dev, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
spiWriteReg(&acc->gyro->dev, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
delay(15);
spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
spiWriteReg(&acc->gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
delay(15);
}

Expand All @@ -103,15 +103,15 @@ void icm20649GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);

spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed

spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
delay(15);
spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
spiWriteReg(&gyro->dev, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
delay(100);
spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
spiWriteReg(&gyro->dev, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;

Expand All @@ -122,19 +122,19 @@ void icm20649GyroInit(gyroDev_t *gyro)
// the ICM20649 only has a single 9KHz DLPF cutoff.
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
raGyroConfigData |= gyro_fsr << 1;
spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
spiWriteReg(&gyro->dev, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
delay(15);
spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
spiWriteReg(&gyro->dev, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
delay(100);

// Data ready interrupt configuration
// back to bank 0
spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
spiWriteReg(&gyro->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4);
delay(15);
spiWriteReg(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
spiWriteReg(&gyro->dev, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);

spiWriteReg(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
spiWriteReg(&gyro->dev, ICM20649_RA_INT_ENABLE_1, 0x01);
}

bool icm20649SpiGyroDetect(gyroDev_t *gyro)
Expand All @@ -154,10 +154,10 @@ bool icm20649SpiGyroDetect(gyroDev_t *gyro)

bool icm20649GyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t data[7];

const bool ack = spiReadWriteBufRB(&gyro->bus, dataToSend, data, 7);
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}
Expand All @@ -173,7 +173,7 @@ bool icm20649AccRead(accDev_t *acc)
{
uint8_t data[6];

const bool ack = spiReadRegMskBufRB(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
const bool ack = spiReadRegMskBufRB(&acc->gyro->dev, ICM20649_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}
Expand Down
16 changes: 9 additions & 7 deletions src/main/sensors/acceleration_init.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,15 @@

#include "acceleration_init.h"

#if !defined(USE_ACC_MPU6000) && !defined(USE_ACC_SPI_MPU6000) && !defined(USE_ACC_MPU6050) && !defined(USE_ACC_MPU6500) && \
!defined(USE_ACC_SPI_MPU6500) && !defined(USE_ACC_SPI_MPU9250) && !defined(USE_ACC_SPI_ICM20602) && \
!defined(USE_ACC_SPI_ICM20689) && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_BMI160) && \
!defined(USE_ACCGYRO_BMI270) && !defined(USE_ACC_SPI_ICM42605) && !defined(USE_ACC_SPI_ICM42688P) && \
!defined(USE_ACC_ADXL345) && !defined(USE_ACC_BMA280) && !defined(USE_ACC_LSM303DLHC) && \
!defined(USE_ACC_MMA8452) && !defined(USE_ACC_LSM303DLHC) && !defined(USE_ACCGYRO_LSM6DSV16X) && \
!defined(USE_VIRTUAL_ACC)
#if !defined(USE_ACC_ADXL345) && !defined(USE_ACC_BMA280) && !defined(USE_ACC_LSM303DLHC) \
&& !defined(USE_ACC_MMA8452) && !defined(USE_ACC_LSM303DLHC) \
&& !defined(USE_ACC_MPU6000) && !defined(USE_ACC_MPU6050) && !defined(USE_ACC_MPU6500) \
&& !defined(USE_ACC_SPI_MPU6000) && !defined(USE_ACC_SPI_MPU6500) && !defined(USE_ACC_SPI_MPU9250) \
&& !defined(USE_ACC_SPI_ICM20602) && !defined(USE_ACC_SPI_ICM20649) && !defined(USE_ACC_SPI_ICM20689) \
&& !defined(USE_ACCGYRO_BMI160) && !defined(USE_ACCGYRO_BMI270) \
&& !defined(USE_ACC_SPI_ICM42605) && !defined(USE_ACC_SPI_ICM42688P) \
&& !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_LSM6DSV16X) \
&& !defined(USE_VIRTUAL_ACC)
#error At least one USE_ACC device definition required
#endif

Expand Down
14 changes: 8 additions & 6 deletions src/main/sensors/gyro_init.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,14 @@
#include "sensors/gyro.h"
#include "sensors/sensors.h"

#if !defined(USE_GYRO_L3G4200D) && !defined(USE_GYRO_MPU3050) && !defined(USE_GYRO_MPU6050) && \
!defined(USE_GYRO_MPU6500) && !defined(USE_GYRO_SPI_ICM20689) && !defined(USE_GYRO_SPI_MPU6000) && \
!defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) && !defined(USE_GYRO_L3GD20) && \
!defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) && \
!defined(USE_ACCGYRO_BMI160) && !defined(USE_ACCGYRO_BMI270) && \
!defined(USE_ACCGYRO_LSM6DSV16X) && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_VIRTUAL_GYRO)
#if !defined(USE_GYRO_L3G4200D) && !defined(USE_GYRO_L3GD20) \
&& !defined(USE_GYRO_MPU3050) && !defined(USE_GYRO_MPU6050) && !defined(USE_GYRO_MPU6500) \
&& !defined(USE_GYRO_SPI_MPU6000) && !defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) \
&& !defined(USE_GYRO_SPI_ICM20602) && !defined(USE_GYRO_SPI_ICM20649) && !defined(USE_GYRO_SPI_ICM20689) \
&& !defined(USE_ACCGYRO_BMI160) && !defined(USE_ACCGYRO_BMI270) \
&& !defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) \
&& !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_LSM6DSV16X) \
&& !defined(USE_VIRTUAL_GYRO)
#error At least one USE_GYRO device definition required
#endif

Expand Down

0 comments on commit 33ead2d

Please sign in to comment.