From 16ca2e11792e46bfd792fbc4fce003ccf965377c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 24 Dec 2022 20:09:29 +0000 Subject: [PATCH] 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 --- .../AP_InertialSensor_BMI088.cpp | 27 ++++++++++++++----- .../AP_InertialSensor_BMI088.h | 2 ++ 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp index 9188e99fb9..2ab475ac27 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.cpp @@ -59,6 +59,9 @@ #define ACCEL_BACKEND_SAMPLE_RATE 1600 #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; AP_InertialSensor_BMI088::AP_InertialSensor_BMI088(AP_InertialSensor &imu, @@ -107,10 +110,10 @@ void AP_InertialSensor_BMI088::start() set_accel_orientation(accel_instance, rotation); // setup callbacks - dev_accel->register_periodic_callback(1000000UL / ACCEL_BACKEND_SAMPLE_RATE, - FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void)); - dev_gyro->register_periodic_callback(1000000UL / GYRO_BACKEND_SAMPLE_RATE, - FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void)); + accel_periodic_handle = dev_accel->register_periodic_callback(ACCEL_BACKEND_PERIOD_US, + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_accel, void)); + gyro_periodic_handle = dev_gyro->register_periodic_callback(GYRO_BACKEND_PERIOD_US, + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI088::read_fifo_gyro, void)); } /* @@ -154,7 +157,8 @@ static const struct { uint8_t reg; uint8_t value; } accel_config[] = { - { REGA_CONF, 0xAC }, + // OSR2 gives 234Hz LPF @ 1.6Khz ODR + { REGA_CONF, 0x9C }, // setup 24g range (16g for BMI085) { REGA_RANGE, 0x03 }, // disable low-power mode @@ -254,8 +258,8 @@ bool AP_InertialSensor_BMI088::gyro_init() return false; } - // setup filter bandwidth 230Hz, no decimation - if (!dev_gyro->write_register(REGG_BW, 0x81, true)) { + // setup filter bandwidth 532Hz, no decimation + if (!dev_gyro->write_register(REGG_BW, 0x80, true)) { return false; } @@ -314,11 +318,16 @@ void AP_InertialSensor_BMI088::read_fifo_accel(void) 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]; if (!read_accel_registers(REGA_FIFO_DATA, data, fifo_length)) { _inc_accel_error_count(accel_instance); return; } + // use new accel_range depending on sensor type const float scale = (1.0/32768.0) * GRAVITY_MSS * accel_range; const uint8_t *p = &data[0]; @@ -407,6 +416,10 @@ void AP_InertialSensor_BMI088::read_fifo_gyro(void) 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)) { _inc_gyro_error_count(gyro_instance); goto check_next; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h index 51b2cdcabc..d8820529aa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI088.h @@ -75,7 +75,9 @@ private: bool setup_accel_config(void); AP_HAL::OwnPtr dev_accel; + AP_HAL::Device::PeriodicHandle accel_periodic_handle; AP_HAL::OwnPtr dev_gyro; + AP_HAL::Device::PeriodicHandle gyro_periodic_handle; uint8_t accel_instance; uint8_t gyro_instance;