From a04d79b8103698e82345b2a25ca48e9a9f65ee50 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Aug 2020 11:07:28 -0400 Subject: [PATCH] bosch/bmi055: minor cleanup and consistency improvements - track consecutive failures and trigger reset aggressively - only count missed drdy interrupts, not time - reset wait times consistent with other drivers --- src/drivers/imu/bosch/bmi055/BMI055.hpp | 5 +- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 52 ++++++++++--------- .../imu/bosch/bmi055/BMI055_Accelerometer.hpp | 8 +-- .../imu/bosch/bmi055/BMI055_Gyroscope.cpp | 49 ++++++++--------- .../imu/bosch/bmi055/BMI055_Gyroscope.hpp | 8 +-- 5 files changed, 63 insertions(+), 59 deletions(-) diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp index 71a68121f0..af0e857ccf 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp @@ -66,10 +66,9 @@ protected: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; hrt_abstime _temperature_update_timestamp{0}; - unsigned _consecutive_failures{0}; - unsigned _total_failures{0}; + int _failure_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_fifo_read_samples{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index a533802b8b..a3d232fe17 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -46,7 +46,7 @@ BMI055_Accelerometer::BMI055_Accelerometer(I2CSPIBusOption bus_option, int bus, _px4_accel(get_device_id(), rotation) { if (drdy_gpio != 0) { - _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval"); + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); } ConfigureSampleRate(_px4_accel.get_max_rate_hz()); @@ -59,7 +59,7 @@ BMI055_Accelerometer::~BMI055_Accelerometer() perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); - perf_free(_drdy_interval_perf); + perf_free(_drdy_missed_perf); } void BMI055_Accelerometer::exit_and_cleanup() @@ -72,14 +72,14 @@ void BMI055_Accelerometer::print_status() { I2CSPIDriverBase::print_status(); - PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_fifo_empty_perf); perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_interval_perf); + perf_print_counter(_drdy_missed_perf); } int BMI055_Accelerometer::probe() @@ -103,8 +103,7 @@ void BMI055_Accelerometer::RunImpl() // BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor. RegisterWrite(Register::BGW_SOFTRESET, 0xB6); _reset_timestamp = now; - _consecutive_failures = 0; - _total_failures = 0; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(25_ms); break; @@ -117,7 +116,7 @@ void BMI055_Accelerometer::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -139,7 +138,7 @@ void BMI055_Accelerometer::RunImpl() _data_ready_interrupt_enabled = true; // backup schedule as a watchdog timeout - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } else { _data_ready_interrupt_enabled = false; @@ -158,7 +157,7 @@ void BMI055_Accelerometer::RunImpl() PX4_DEBUG("Configure failed, retrying"); } - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } break; @@ -166,8 +165,8 @@ void BMI055_Accelerometer::RunImpl() case STATE::FIFO_READ: { if (_data_ready_interrupt_enabled) { // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_accel_samples) { - perf_count_interval(_drdy_interval_perf, now); + if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -186,7 +185,7 @@ void BMI055_Accelerometer::RunImpl() const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter; if (fifo_frame_counter > FIFO_MAX_SAMPLES) { - // not necessarily an actual FIFO overflow, but more samples than we expected or can publish + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); @@ -196,23 +195,25 @@ void BMI055_Accelerometer::RunImpl() } else if (fifo_frame_counter >= 1) { if (FIFORead(now, fifo_frame_counter)) { success = true; - _consecutive_failures = 0; + + if (_failure_count > 0) { + _failure_count--; + } } } } if (!success) { - _consecutive_failures++; - _total_failures++; + _failure_count++; // full reset if things are failing consistently - if (_consecutive_failures > 100 || _total_failures > 1000) { + if (_failure_count > 10) { Reset(); return; } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -274,12 +275,12 @@ void BMI055_Accelerometer::ConfigureSampleRate(int sample_rate) const float min_interval = FIFO_SAMPLE_DT; _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); - _fifo_accel_samples = math::min((float)_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); + _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); - // recompute FIFO empty interval (us) with actual accel sample limit - _fifo_empty_interval_us = _fifo_accel_samples * (1e6f / ACCEL_RATE); + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); - ConfigureFIFOWatermark(_fifo_accel_samples); + ConfigureFIFOWatermark(_fifo_samples); } void BMI055_Accelerometer::ConfigureFIFOWatermark(uint8_t samples) @@ -322,9 +323,9 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi void BMI055_Accelerometer::DataReady() { - uint8_t expected = 0; + uint32_t expected = 0; - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_accel_samples)) { + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { ScheduleNow(); } } @@ -377,7 +378,7 @@ uint8_t BMI055_Accelerometer::RegisterRead(Register reg) void BMI055_Accelerometer::RegisterWrite(Register reg, uint8_t value) { - uint8_t cmd[2] {(uint8_t)reg, value}; + uint8_t cmd[2] { (uint8_t)reg, value }; transfer(cmd, cmd, sizeof(cmd)); } @@ -460,6 +461,9 @@ void BMI055_Accelerometer::UpdateTemperature() if (PX4_ISFINITE(temperature)) { _px4_accel.set_temperature(temperature); + + } else { + perf_count(_bad_transfer_perf); } } diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp index fd5eb748f6..80ef7831b2 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp @@ -56,8 +56,8 @@ private: void exit_and_cleanup() override; // Sensor Configuration - static constexpr uint32_t ACCEL_RATE{2000}; // 2000 Hz accel - static constexpr float FIFO_SAMPLE_DT{1e6f / ACCEL_RATE}; + static constexpr uint32_t RATE{2000}; // 2000 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; @@ -105,9 +105,9 @@ private: perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")}; perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")}; perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")}; - perf_counter_t _drdy_interval_perf{nullptr}; + perf_counter_t _drdy_missed_perf{nullptr}; - uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{7}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index 9fe772ffd4..232d2d2ffa 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -46,7 +46,7 @@ BMI055_Gyroscope::BMI055_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t _px4_gyro(get_device_id(), rotation) { if (drdy_gpio != 0) { - _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval"); + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); } ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); @@ -59,7 +59,7 @@ BMI055_Gyroscope::~BMI055_Gyroscope() perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); - perf_free(_drdy_interval_perf); + perf_free(_drdy_missed_perf); } void BMI055_Gyroscope::exit_and_cleanup() @@ -72,14 +72,14 @@ void BMI055_Gyroscope::print_status() { I2CSPIDriverBase::print_status(); - PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_fifo_empty_perf); perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_interval_perf); + perf_print_counter(_drdy_missed_perf); } int BMI055_Gyroscope::probe() @@ -103,8 +103,7 @@ void BMI055_Gyroscope::RunImpl() // BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor. RegisterWrite(Register::BGW_SOFTRESET, 0xB6); _reset_timestamp = now; - _consecutive_failures = 0; - _total_failures = 0; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(25_ms); break; @@ -117,7 +116,7 @@ void BMI055_Gyroscope::RunImpl() } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; ScheduleDelayed(100_ms); @@ -139,7 +138,7 @@ void BMI055_Gyroscope::RunImpl() _data_ready_interrupt_enabled = true; // backup schedule as a watchdog timeout - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } else { _data_ready_interrupt_enabled = false; @@ -158,7 +157,7 @@ void BMI055_Gyroscope::RunImpl() PX4_DEBUG("Configure failed, retrying"); } - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } break; @@ -166,8 +165,8 @@ void BMI055_Gyroscope::RunImpl() case STATE::FIFO_READ: { if (_data_ready_interrupt_enabled) { // scheduled from interrupt if _drdy_fifo_read_samples was set - if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) { - perf_count_interval(_drdy_interval_perf, now); + if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -186,7 +185,7 @@ void BMI055_Gyroscope::RunImpl() const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter; if (fifo_frame_counter > FIFO_MAX_SAMPLES) { - // not necessarily an actual FIFO overflow, but more samples than we expected or can publish + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); @@ -196,23 +195,25 @@ void BMI055_Gyroscope::RunImpl() } else if (fifo_frame_counter >= 1) { if (FIFORead(now, fifo_frame_counter)) { success = true; - _consecutive_failures = 0; + + if (_failure_count > 0) { + _failure_count--; + } } } } if (!success) { - _consecutive_failures++; - _total_failures++; + _failure_count++; // full reset if things are failing consistently - if (_consecutive_failures > 100 || _total_failures > 1000) { + if (_failure_count > 10) { Reset(); return; } } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { _last_config_check_timestamp = now; @@ -272,12 +273,12 @@ void BMI055_Gyroscope::ConfigureSampleRate(int sample_rate) const float min_interval = FIFO_SAMPLE_DT; _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); - _fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES); + _fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); - // recompute FIFO empty interval (us) with actual gyro sample limit - _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); - ConfigureFIFOWatermark(_fifo_gyro_samples); + ConfigureFIFOWatermark(_fifo_samples); } void BMI055_Gyroscope::ConfigureFIFOWatermark(uint8_t samples) @@ -320,9 +321,9 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a void BMI055_Gyroscope::DataReady() { - uint8_t expected = 0; + uint32_t expected = 0; - if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) { ScheduleNow(); } } @@ -375,7 +376,7 @@ uint8_t BMI055_Gyroscope::RegisterRead(Register reg) void BMI055_Gyroscope::RegisterWrite(Register reg, uint8_t value) { - uint8_t cmd[2] {(uint8_t)reg, value}; + uint8_t cmd[2] { (uint8_t)reg, value }; transfer(cmd, cmd, sizeof(cmd)); } diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp index 1b03ef3eb2..54331abdc2 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp @@ -56,8 +56,8 @@ private: void exit_and_cleanup() override; // Sensor Configuration - static constexpr uint32_t GYRO_RATE{2000}; // 2000 Hz gyro - static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_RATE}; + static constexpr uint32_t RATE{2000}; // 2000 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; @@ -103,9 +103,9 @@ private: perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")}; perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")}; perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")}; - perf_counter_t _drdy_interval_perf{nullptr}; + perf_counter_t _drdy_missed_perf{nullptr}; - uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{8};