Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use 234Hz accel LPF and 532Hz gyro LPF on BMI088 to more closely match Invensense 

synchronize fifo reads with backend update on BMI088
This commit is contained in:
Andy Piper 2022-12-24 20:09:29 +00:00 committed by Andrew Tridgell
parent d29ccba3f7
commit 16ca2e1179
2 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp
View File

@ -59,6 +59,9 @@
#define ACCEL_BACKEND_SAMPLE_RATE 1600 #define ACCEL_BACKEND_SAMPLE_RATE 1600
#define GYRO_BACKEND_SAMPLE_RATE 2000 #define GYRO_BACKEND_SAMPLE_RATE 2000
const uint32_t ACCEL_BACKEND_PERIOD_US = 1000000UL / ACCEL_BACKEND_SAMPLE_RATE;
const uint32_t GYRO_BACKEND_PERIOD_US = 1000000UL / GYRO_BACKEND_SAMPLE_RATE;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_InertialSensor_BMI088::AP_InertialSensor_BMI088(AP_InertialSensor &imu, AP_InertialSensor_BMI088::AP_InertialSensor_BMI088(AP_InertialSensor &imu,
@ -107,9 +110,9 @@ void AP_InertialSensor_BMI088::start()
set_accel_orientation(accel_instance, rotation); set_accel_orientation(accel_instance, rotation);
// setup callbacks // setup callbacks
dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE, accel_periodic_handle = dev_accel->register_periodic_callback(ACCEL_BACKEND_PERIOD_US,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void)); FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void));
dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE, gyro_periodic_handle = dev_gyro->register_periodic_callback(GYRO_BACKEND_PERIOD_US,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void)); FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void));
} }
@ -154,7 +157,8 @@ static const struct {
uint8_t reg; uint8_t reg;
uint8_t value; uint8_t value;
} accel_config[] = { } accel_config[] = {
{ REGA_CONF, 0xAC }, // OSR2 gives 234Hz LPF @ 1.6Khz ODR
{ REGA_CONF, 0x9C },
// setup 24g range (16g for BMI085) // setup 24g range (16g for BMI085)
{ REGA_RANGE, 0x03 }, { REGA_RANGE, 0x03 },
// disable low-power mode // disable low-power mode
@ -254,8 +258,8 @@ bool AP_InertialSensor_BMI088::gyro_init()
return false; return false;
} }
// setup filter bandwidth 230Hz, no decimation // setup filter bandwidth 532Hz, no decimation
if (!dev_gyro->write_register(REGG_BW, 0x81, true)) { if (!dev_gyro->write_register(REGG_BW, 0x80, true)) {
return false; return false;
} }
@ -314,11 +318,16 @@ void AP_InertialSensor_BMI088::read_fifo_accel(void)
return; return;
} }
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo_accel() without updating the sensor data
dev_accel->adjust_periodic_callback(accel_periodic_handle, ACCEL_BACKEND_PERIOD_US);
uint8_t data[fifo_length]; uint8_t data[fifo_length];
if (!read_accel_registers(REGA_FIFO_DATA, data, fifo_length)) { if (!read_accel_registers(REGA_FIFO_DATA, data, fifo_length)) {
_inc_accel_error_count(accel_instance); _inc_accel_error_count(accel_instance);
return; return;
} }
// use new accel_range depending on sensor type // use new accel_range depending on sensor type
const float scale = (1.0/32768.0) * GRAVITY_MSS * accel_range; const float scale = (1.0/32768.0) * GRAVITY_MSS * accel_range;
const uint8_t *p = &data[0]; const uint8_t *p = &data[0];
@ -407,6 +416,10 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void)
goto check_next; goto check_next;
} }
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo_gyro() without updating the sensor data
dev_gyro->adjust_periodic_callback(gyro_periodic_handle, GYRO_BACKEND_PERIOD_US);
if (!dev_gyro->read_registers(REGG_FIFO_DATA, (uint8_t *)data, num_frames*6)) { if (!dev_gyro->read_registers(REGG_FIFO_DATA, (uint8_t *)data, num_frames*6)) {
_inc_gyro_error_count(gyro_instance); _inc_gyro_error_count(gyro_instance);
goto check_next; goto check_next;

2
libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h
View File

@ -75,7 +75,9 @@ private:
bool setup_accel_config(void); bool setup_accel_config(void);
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel; AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
AP_HAL::Device::PeriodicHandle accel_periodic_handle;
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro; AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
AP_HAL::Device::PeriodicHandle gyro_periodic_handle;
uint8_t accel_instance; uint8_t accel_instance;
uint8_t gyro_instance; uint8_t gyro_instance;