From 8855a54720782629908fc59f70237eb3783466fd Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Fri, 22 Jul 2022 20:56:20 +0100 Subject: [PATCH] AP_InertialSensor: use filtered data in BMI270 and implement fifo reset increase gyro ODR to 3.2Khz to allow higher AAF use OSR4 on accel filter to achieve nominal 188Hz --- .../AP_InertialSensor_BMI270.cpp | 58 ++++++++++++++----- .../AP_InertialSensor_BMI270.h | 7 ++- 2 files changed, 47 insertions(+), 18 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp index 2dac636efe..a2489affab 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.cpp @@ -210,7 +210,6 @@ void AP_InertialSensor_BMI270::start() _dev->get_semaphore()->give(); - // using headerless mode so gyro and accel ODRs must match if (!_imu.register_accel(_accel_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270)) || !_imu.register_gyro(_gyro_instance, BMI270_BACKEND_SAMPLE_RATE, _dev->get_bus_id_devtype(DEVTYPE_BMI270))) { return; @@ -318,8 +317,10 @@ void AP_InertialSensor_BMI270::check_err_reg() void AP_InertialSensor_BMI270::configure_accel() { - // set acc in high performance mode with normal filtering at 1600Hz - write_register(BMI270_REG_ACC_CONF, 1U<<7 | 0x2U<<4 | 0x0C); + // set acc in high performance mode with OSR4 filtering (751Hz/4) at 1600Hz + // see https://community.bosch-sensortec.com/t5/MEMS-sensors-forum/BMI270-OSR-mode-behaviour/td-p/52020 + // OSR4 is a 188Hz filter cutoff, acc_bwp == 0, equivalent to other driver filters + write_register(BMI270_REG_ACC_CONF, 1U<<7 | 0x0C); // set acc to 16G full scale write_register(BMI270_REG_ACC_RANGE, 0x03); @@ -328,8 +329,9 @@ void AP_InertialSensor_BMI270::configure_accel() void AP_InertialSensor_BMI270::configure_gyro() { - // set gyro in high performance filter mode, high performance noise mode. normal filtering at 1600Hz - write_register(BMI270_REG_GYRO_CONF, 1U<<7 | 1<<6 | 2<<4 | 0x0C); + // set gyro in high performance filter mode, high performance noise mode, normal filtering at 3.2KHz + // filter cutoff 751hz + write_register(BMI270_REG_GYRO_CONF, 1U<<7 | 1<<6 | 2<<4 | 0x0D); // set gyro to 2000dps full scale // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) @@ -340,27 +342,44 @@ void AP_InertialSensor_BMI270::configure_gyro() void AP_InertialSensor_BMI270::configure_fifo() { - // don't stop when full, disable sensortime frame - write_register(BMI270_REG_FIFO_CONFIG_0, 0x00); + // stop when full, disable sensortime frame + write_register(BMI270_REG_FIFO_CONFIG_0, 0x01); // accel + gyro data in FIFO together with headers write_register(BMI270_REG_FIFO_CONFIG_1, 1U<<7 | 1U<<6 | 1U<<4); - // unfiltered with gyro downsampled by 2^2 to give 1600Hz - write_register(BMI270_REG_FIFO_DOWNS, 0x02); + // filtered data downsampled by 2**1 to 1600Hz + write_register(BMI270_REG_FIFO_DOWNS, 1U<<7 | 1U<<3 | 0x01); // disable advanced power save, enable FIFO self-wake write_register(BMI270_REG_PWR_CONF, 0x02); // Enable the gyro, accelerometer and temperature sensor - disable aux interface write_register(BMI270_REG_PWR_CTRL, 0x0E); - // flush FIFO - write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH); + + fifo_reset(); check_err_reg(); } +void AP_InertialSensor_BMI270::fifo_reset() +{ + // flush and reset FIFO + write_register(BMI270_REG_CMD, BMI270_CMD_FIFOFLUSH); + + notify_accel_fifo_reset(_accel_instance); + notify_gyro_fifo_reset(_gyro_instance); +} + /* read fifo */ void AP_InertialSensor_BMI270::read_fifo(void) { + // check for FIFO errors/overflow + uint8_t err = 0; + read_registers(BMI270_REG_ERR_REG, &err, 1); + if ((err>>6 & 1) == 1) { + fifo_reset(); + return; + } + uint8_t len[2]; if (!read_registers(BMI270_REG_FIFO_LENGTH_LSB, len, 2)) { _inc_accel_error_count(_accel_instance); @@ -418,17 +437,22 @@ void AP_InertialSensor_BMI270::read_fifo(void) break; case 0x48: // fifo config frame - frame_len = 2; + frame_len = 5; break; case 0x50: // sample drop frame frame_len = 2; break; + case 0x80: + // invalid frame + fifo_reset(); + return; } p += frame_len; fifo_length -= frame_len; } + // temperature sensor updated every 10ms if (temperature_counter++ == 100) { temperature_counter = 0; uint8_t tbuf[2]; @@ -436,10 +460,12 @@ void AP_InertialSensor_BMI270::read_fifo(void) _inc_accel_error_count(_accel_instance); _inc_gyro_error_count(_gyro_instance); } else { - uint16_t temp_uint11 = (tbuf[0]<<3) | (tbuf[1]>>5); - int16_t temp_int11 = temp_uint11>1023?temp_uint11-2048:temp_uint11; - float temp_degc = temp_int11 * 0.125f + 23; - _publish_temperature(_accel_instance, temp_degc); + uint16_t tval = tbuf[0] | (tbuf[1] << 8); + if (tval != 0x8000) { // 0x8000 is invalid + int16_t klsb = static_cast(tval); + float temp_degc = klsb * 0.002f + 23.0f; + _publish_temperature(_accel_instance, temp_degc); + } } } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h index 4879832f01..bc7eb88fb1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI270.h @@ -93,10 +93,13 @@ private: */ void configure_gyro(); + /** + * Reset FIFO. + */ + void fifo_reset(); + /** * Configure FIFO. - * - * @return true on success, false otherwise. */ void configure_fifo();