Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 02:58:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added BMI088 accel config retry

Browse Source
This commit is contained in:
Andrew Tridgell 2019-03-15 14:32:41 +11:00
parent 928d7c7e71
commit fa63243eb0
2 changed files with 51 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -146,6 +146,44 @@ bool AP_InertialSensor_BMI088::write_accel_register(uint8_t reg, uint8_t v)
return false;
}
static const struct {
uint8_t reg;
uint8_t value;
} accel_config[] = {
{ REGA_CONF, 0xAC },
// setup 24g range
{ REGA_RANGE, 0x03 },
// disable low-power mode
{ REGA_PWR_CONF, 0 },
{ REGA_PWR_CTRL, 0x04 },
// setup FIFO for streaming X,Y,Z
{ REGA_FIFO_CONFIG0, 0x00 },
{ REGA_FIFO_CONFIG1, 0x50 },
};
bool AP_InertialSensor_BMI088::setup_accel_config(void)
{
if (done_accel_config) {
return true;
}
accel_config_count++;
for (uint8_t i=0; i<ARRAY_SIZE(accel_config); i++) {
uint8_t v;
if (!read_accel_registers(accel_config[i].reg, &v, 1)) {
return false;
}
if (v == accel_config[i].value) {
continue;
}
if (!write_accel_register(accel_config[i].reg, accel_config[i].value)) {
return false;
}
}
done_accel_config = true;
hal.console->printf("BMI088: accel config OK (%u tries)\n", accel_config_count);
return true;
}
/*
probe and initialise accelerometer
*/
@ -162,30 +200,8 @@ bool AP_InertialSensor_BMI088::accel_init()
return false;
}
// setup normal mode for DLPF, with 1600Hz ODR
if (!write_accel_register(REGA_CONF, 0xAC)) {
return false;
}
// setup 24g range
if (!write_accel_register(REGA_RANGE, 0x03)) {
return false;
}
// disable low-power mode
if (!write_accel_register(REGA_PWR_CONF, 0)) {
return false;
}
if (!write_accel_register(REGA_PWR_CTRL, 0x04)) {
return false;
}
// setup FIFO for streaming X,Y,Z
if (!write_accel_register(REGA_FIFO_CONFIG0, 0x00)) {
return false;
}
if (!write_accel_register(REGA_FIFO_CONFIG1, 0x50)) {
return false;
if (!setup_accel_config()) {
hal.console->printf("BMI088: delaying accel config\n");
}
hal.console->printf("BMI088: found accel\n");
@ -255,6 +271,9 @@ bool AP_InertialSensor_BMI088::init()
*/
void AP_InertialSensor_BMI088::read_fifo_accel(void)
{
if (!setup_accel_config()) {
return;
}
uint8_t len[2];
if (!read_accel_registers(REGA_FIFO_LEN0, len, 2)) {
_inc_accel_error_count(accel_instance);

8
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
View File

@ -69,6 +69,11 @@ private:
*/
bool write_accel_register(uint8_t reg, uint8_t v);
/*
configure accel registers
*/
bool setup_accel_config(void);
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
@ -76,4 +81,7 @@ private:
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t temperature_counter;
bool done_accel_config;
uint32_t accel_config_count;
};