Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

BMI160 gyro update

Browse source
This commit is contained in:
Martin Budden 2017-08-25 08:34:24 +01:00
parent 2f0a678cbe
commit 7c702fd6ae
1 changed files with 19 additions and 60 deletions
Download patch file Download diff file Expand all files Collapse all files

79
src/main/drivers/accgyro/accgyro_spi_bmi160.c
View file

@ -90,7 +90,6 @@ static volatile bool bmi160ExtiInitDone = false;
//! Private functions //! Private functions
static int32_t BMI160_Config(const busDevice_t *bus); static int32_t BMI160_Config(const busDevice_t *bus);
static int32_t BMI160_do_foc(const busDevice_t *bus); static int32_t BMI160_do_foc(const busDevice_t *bus);
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
uint8_t bmi160Detect(const busDevice_t *bus) uint8_t bmi160Detect(const busDevice_t *bus)
{ {
@ -106,7 +105,7 @@ uint8_t bmi160Detect(const busDevice_t *bus)
/* Read this address to activate SPI (see p. 84) */ /* Read this address to activate SPI (see p. 84) */
spiBusReadRegister(bus, 0x7F); spiBusReadRegister(bus, 0x7F);
delay(10); // Give SPI some time to start up delay(100); // Give SPI some time to start up
/* Check the chip ID */ /* Check the chip ID */
if (spiBusReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) { if (spiBusReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) {
@ -151,14 +150,10 @@ static int32_t BMI160_Config(const busDevice_t *bus)
{ {
// Set normal power mode for gyro and accelerometer // Set normal power mode for gyro and accelerometer
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0) { spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
return -1;
}
delay(100); // can take up to 80ms delay(100); // can take up to 80ms
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0) { spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
return -2;
}
delay(5); // can take up to 3.8ms delay(5); // can take up to 3.8ms
// Verify that normal power mode was entered // Verify that normal power mode was entered
@ -169,49 +164,33 @@ static int32_t BMI160_Config(const busDevice_t *bus)
// Set odr and ranges // Set odr and ranges
// Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used // Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0) { spiBusWriteRegister(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
return -3;
}
delay(1); delay(1);
// Set gyr_bwp = 0b010 so only the first filter stage is used // Set gyr_bwp = 0b010 so only the first filter stage is used
if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0) { spiBusWriteRegister(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz);
return -4;
}
delay(1); delay(1);
if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0) { spiBusWriteRegister(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
return -5;
}
delay(1); delay(1);
if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0) { spiBusWriteRegister(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
return -6;
}
delay(1); delay(1);
// Enable offset compensation // Enable offset compensation
uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0); uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0);
if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0) { spiBusWriteRegister(bus, BMI160_REG_OFFSET_0, val | 0xC0);
return -7;
}
// Enable data ready interrupt // Enable data ready interrupt
if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0) { spiBusWriteRegister(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
return -8;
}
delay(1); delay(1);
// Enable INT1 pin // Enable INT1 pin
if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0) { spiBusWriteRegister(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
return -9;
}
delay(1); delay(1);
// Map data ready interrupt to INT1 pin // Map data ready interrupt to INT1 pin
if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0) { spiBusWriteRegister(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
return -10;
}
delay(1); delay(1);
return 0; return 0;
@ -221,14 +200,10 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
{ {
// assume sensor is mounted on top // assume sensor is mounted on top
uint8_t val = 0x7D;; uint8_t val = 0x7D;;
if (BMI160_WriteReg(bus, BMI160_REG_FOC_CONF, val) != 0) { spiBusWriteRegister(bus, BMI160_REG_FOC_CONF, val);
return -1;
}
// Start FOC // Start FOC
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) { spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC);
return -2;
}
// Wait for FOC to complete // Wait for FOC to complete
for (int i=0; i<50; i++) { for (int i=0; i<50; i++) {
@ -244,13 +219,9 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
// Program NVM // Program NVM
val = spiBusReadRegister(bus, BMI160_REG_CONF); val = spiBusReadRegister(bus, BMI160_REG_CONF);
if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) { spiBusWriteRegister(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
return -4;
}
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) { spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
return -5;
}
// Wait for NVM programming to complete // Wait for NVM programming to complete
for (int i=0; i<50; i++) { for (int i=0; i<50; i++) {
@ -267,18 +238,6 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
return 0; return 0;
} }
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(busdev->busdev_u.spi.csnPin); // Enable
spiTransferByte(BMI160_SPI_INSTANCE, 0x7f & reg);
spiTransferByte(BMI160_SPI_INSTANCE, data);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
return 0;
}
extiCallbackRec_t bmi160IntCallbackRec; extiCallbackRec_t bmi160IntCallbackRec;
void bmi160ExtiHandler(extiCallbackRec_t *cb) void bmi160ExtiHandler(extiCallbackRec_t *cb)
@ -366,13 +325,13 @@ bool bmi160GyroRead(gyroDev_t *gyro)
void bmi160SpiGyroInit(gyroDev_t *gyro) void bmi160SpiGyroInit(gyroDev_t *gyro)
{ {
BMI160_Init(gyro->bus.busdev_u.spi.csnPin); BMI160_Init(&gyro->bus);
bmi160IntExtiInit(gyro); bmi160IntExtiInit(gyro);
} }
void bmi160SpiAccInit(accDev_t *acc) void bmi160SpiAccInit(accDev_t *acc)
{ {
BMI160_Init(acc->bus.busdev_u.spi.csnPin); BMI160_Init(&acc->bus);
acc->acc_1G = 512 * 8; acc->acc_1G = 512 * 8;
} }
@ -380,7 +339,7 @@ void bmi160SpiAccInit(accDev_t *acc)
bool bmi160SpiAccDetect(accDev_t *acc) bool bmi160SpiAccDetect(accDev_t *acc)
{ {
if (bmi160Detect(acc->bus.busdev_u.spi.csnPin) == MPU_NONE) { if (bmi160Detect(&acc->bus) == MPU_NONE) {
return false; return false;
} }
@ -393,7 +352,7 @@ bool bmi160SpiAccDetect(accDev_t *acc)
bool bmi160SpiGyroDetect(gyroDev_t *gyro) bool bmi160SpiGyroDetect(gyroDev_t *gyro)
{ {
if (bmi160Detect(gyro->bus.busdev_u.spi.csnPin) == MPU_NONE) { if (bmi160Detect(&gyro->bus) == MPU_NONE) {
return false; return false;
} }