Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Fix gyro SpiDetect routines

Browse source
This commit is contained in:
Steve Evans 2022-01-09 03:50:16 +00:00
parent 18ee33e7a8
commit d3c3d5fa90
5 changed files with 20 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/accgyro/accgyro_spi_bmi160.h
View file

@ -68,6 +68,6 @@ enum bmi160_gyro_range {
BMI160_RANGE_2000DPS = 0x00, BMI160_RANGE_2000DPS = 0x00,
}; };
uint8_t bmi160Detect(const busDevice_t *bus); uint8_t bmi160Detect(const extDevice_t *dev);
bool bmi160SpiAccDetect(accDev_t *acc); bool bmi160SpiAccDetect(accDev_t *acc);
bool bmi160SpiGyroDetect(gyroDev_t *gyro); bool bmi160SpiGyroDetect(gyroDev_t *gyro);

19
src/main/drivers/accgyro/accgyro_spi_icm20649.c
View file

@ -42,7 +42,7 @@
// 8 MHz max SPI frequency // 8 MHz max SPI frequency
#define ICM20649_MAX_SPI_CLK_HZ 8000000 #define ICM20649_MAX_SPI_CLK_HZ 8000000
static void icm20649SpiInit(const busDevice_t *bus) static void icm20649SpiInit(const extDevice_t *dev)
{ {
static bool hardwareInitialised = false; static bool hardwareInitialised = false;
@ -57,16 +57,17 @@ static void icm20649SpiInit(const busDevice_t *bus)
hardwareInitialised = true; hardwareInitialised = true;
} }
uint8_t icm20649SpiDetect(const busDevice_t *bus) uint8_t icm20649SpiDetect(const extDevice_t *dev)
{ {
icm20649SpiInit(bus);
icm20649SpiInit(dev);
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiWriteReg(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe spiWriteReg(dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
delay(15); delay(15);
spiWriteReg(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); spiWriteReg(dev, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
uint8_t icmDetected = MPU_NONE; uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20; uint8_t attemptsRemaining = 20;
@ -96,14 +97,14 @@ void icm20649AccInit(accDev_t *acc)
// 1,024 LSB/g 30g // 1,024 LSB/g 30g
acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048; acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); spiSetClkDivisor(&acc->dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15); delay(15);
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G; const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
spiWriteReg(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); spiWriteReg(&acc->dev, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
delay(15); delay(15);
spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 spiWriteReg(&acc->dev, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
delay(15); delay(15);
} }

2
src/main/drivers/accgyro/accgyro_spi_icm20649.h
View file

@ -59,7 +59,7 @@ enum icm20649_accel_fsr_e {
void icm20649AccInit(accDev_t *acc); void icm20649AccInit(accDev_t *acc);
void icm20649GyroInit(gyroDev_t *gyro); void icm20649GyroInit(gyroDev_t *gyro);
uint8_t icm20649SpiDetect(const busDevice_t *bus); uint8_t icm20649SpiDetect(const extDevice_t *dev);
bool icm20649SpiAccDetect(accDev_t *acc); bool icm20649SpiAccDetect(accDev_t *acc);
bool icm20649SpiGyroDetect(gyroDev_t *gyro); bool icm20649SpiGyroDetect(gyroDev_t *gyro);

15
src/main/drivers/accgyro/accgyro_spi_l3gd20.c
View file

@ -101,18 +101,17 @@ static void l3gd20IntExtiInit(gyroDev_t *gyro)
void l3gd20GyroInit(gyroDev_t *gyro) void l3gd20GyroInit(gyroDev_t *gyro)
{ {
spiSetClkDivisor(dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ)); spiSetClkDivisor(&gyro->dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ));
spiWriteReg(&gyro->bus, CTRL_REG5_ADDR, BOOT); spiWriteReg(&gyro->dev, CTRL_REG5_ADDR, BOOT);
delayMicroseconds(100); delayMicroseconds(100);
spiWriteReg(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3); spiWriteReg(&gyro->dev, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
//spiWriteReg(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4);
delayMicroseconds(1); delayMicroseconds(1);
spiWriteReg(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000); spiWriteReg(&gyro->dev, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
delay(100); delay(100);
@ -126,7 +125,7 @@ static bool l3gd20GyroRead(gyroDev_t *gyro)
{ {
uint8_t buf[6]; uint8_t buf[6];
const bool ack = spiReadRegMskBufRB(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf)); const bool ack = spiReadRegMskBufRB(&gyro->dev, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf));
if (!ack) { if (!ack) {
return false; return false;
} }
@ -141,9 +140,9 @@ static bool l3gd20GyroRead(gyroDev_t *gyro)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f #define L3GD20_GYRO_SCALE_FACTOR 0.07f
uint8_t l3gd20Detect(const busDevice_t *bus) uint8_t l3gd20Detect(const extDevice_t *dev)
{ {
UNUSED(bus); UNUSED(dev);
return L3GD20_SPI; // blindly assume it's present, for now. return L3GD20_SPI; // blindly assume it's present, for now.
} }

2
src/main/drivers/accgyro/accgyro_spi_l3gd20.h
View file

@ -20,5 +20,5 @@
#pragma once #pragma once
uint8_t l3gd20Detect(const busDevice_t *bus); uint8_t l3gd20Detect(const extDevice_t *dev);
bool l3gd20GyroDetect(gyroDev_t *gyro); bool l3gd20GyroDetect(gyroDev_t *gyro);