Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

icm20608g: accumulated minor improvements and cleanup

Browse Source 
- update copyright year
 - perform reset as per the datasheet (disable I2C immediately, set power mode, wait for appropriate time, etc)
 - only track consecutive errors (not total) to trigger full reset if necessary
 - remove interrupt perf counter and instead only count misses
 - minor style changes to stay in sync with the other Invensense drivers
This commit is contained in:
Daniel Agar
2020-07-09 11:27:37 -04:00
parent e3b1315df5
commit 8c8677758a
3 changed files with 30 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
+22 -22
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -49,7 +49,7 @@ ICM20608G::ICM20608G(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{ {
if (drdy_gpio != 0) { if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval"); _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
} }
ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
@@ -62,7 +62,7 @@ ICM20608G::~ICM20608G()
perf_free(_fifo_empty_perf); perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf); perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf); perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf); perf_free(_drdy_missed_perf);
} }
int ICM20608G::init() int ICM20608G::init()
@@ -96,15 +96,14 @@ void ICM20608G::print_status()
{ {
I2CSPIDriverBase::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_register_perf);
perf_print_counter(_bad_transfer_perf); perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf); perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf); perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf); perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf); perf_print_counter(_drdy_missed_perf);
} }
int ICM20608G::probe() int ICM20608G::probe()
@@ -128,8 +127,7 @@ void ICM20608G::RunImpl()
// PWR_MGMT_1: Device Reset // PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = now; _reset_timestamp = now;
_consecutive_failures = 0; _failure_count = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET; _state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(100_ms); ScheduleDelayed(100_ms);
break; break;
@@ -142,9 +140,9 @@ void ICM20608G::RunImpl()
&& (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) {
// Wakeup and reset digital signal path // Wakeup and reset digital signal path
RegisterWrite(Register::PWR_MGMT_1, 0); RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); RegisterWrite(Register::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST);
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST, USER_CTRL_BIT::I2C_IF_DIS); RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS);
// if reset succeeded then configure // if reset succeeded then configure
_state = STATE::CONFIGURE; _state = STATE::CONFIGURE;
@@ -193,7 +191,7 @@ void ICM20608G::RunImpl()
PX4_DEBUG("Configure failed, retrying"); PX4_DEBUG("Configure failed, retrying");
} }
ScheduleDelayed(10_ms); ScheduleDelayed(100_ms);
} }
break; break;
@@ -201,8 +199,8 @@ void ICM20608G::RunImpl()
case STATE::FIFO_READ: { case STATE::FIFO_READ: {
if (_data_ready_interrupt_enabled) { if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set // scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) { if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count_interval(_drdy_interval_perf, now); perf_count(_drdy_missed_perf);
} }
// push backup schedule back // push backup schedule back
@@ -233,23 +231,25 @@ void ICM20608G::RunImpl()
} else if (samples >= 1) { } else if (samples >= 1) {
if (FIFORead(now, samples)) { if (FIFORead(now, samples)) {
success = true; success = true;
_consecutive_failures = 0;
if (_failure_count > 0) {
_failure_count--;
}
} }
} }
} }
if (!success) { if (!success) {
_consecutive_failures++; _failure_count++;
_total_failures++;
// full reset if things are failing consistently // full reset if things are failing consistently
if (_consecutive_failures > 100 || _total_failures > 1000) { if (_failure_count > 10) {
Reset(); Reset();
return; 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 // check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) { if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now; _last_config_check_timestamp = now;
@@ -375,12 +375,12 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20608G::DataReady() void ICM20608G::DataReady()
{ {
const uint8_t count = _drdy_count.fetch_add(1) + 1; uint32_t expected = 0;
uint8_t expected = 0;
// at least the required number of samples in the FIFO // at least the required number of samples in the FIFO
if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0); _drdy_count.store(0);
ScheduleNow(); ScheduleNow();
} }
src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
+7 -8
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -133,16 +133,15 @@ private:
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_interval_perf{nullptr}; perf_counter_t _drdy_missed_perf{nullptr};
hrt_abstime _reset_timestamp{0}; hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0}; hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0}; hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0}; int _failure_count{0};
unsigned _total_failures{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0}; px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0}; px4::atomic<uint32_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false}; bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t { enum class STATE : uint8_t {
@@ -155,7 +154,7 @@ private:
STATE _state{STATE::RESET}; STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0}; uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{9}; static constexpr uint8_t size_register_cfg{9};
@@ -169,6 +168,6 @@ private:
{ Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 }, { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }, { Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
}; };
}; };
src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp
+1 -1
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2019 PX4 Development Team. All rights reserved. * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions