forked from Archive/PX4-Autopilot
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
This commit is contained in:
Daniel Agar
parent
08ad7850fc
commit
a04d79b810
|
|
@ -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<uint8_t> _drdy_fifo_read_samples{0};
|
||||
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
@ -460,6 +461,9 @@ void BMI055_Accelerometer::UpdateTemperature()
|
|||
|
||||
if (PX4_ISFINITE(temperature)) {
|
||||
_px4_accel.set_temperature(temperature);
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
|
||||
uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{7};
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{8};
|
||||
|
|
|
|||
Loading…
Reference in New Issue