diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt index cd14e15674..baedef2659 100644 --- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,6 +35,7 @@ px4_add_module( MODULE drivers__imu__invensense__icm20602 MAIN icm20602 COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS ICM20602.cpp ICM20602.hpp diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 72e63f7474..c40982e2c7 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -74,7 +74,21 @@ int ICM20602::init() return ret; } - return Reset() ? 0 : -1; + _state = STATE::CHECK_CONFIGURATION; + ScheduleNow(); + return 0; +} + +int ICM20602::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; } bool ICM20602::Reset() @@ -106,18 +120,6 @@ void ICM20602::print_status() perf_print_counter(_drdy_missed_perf); } -int ICM20602::probe() -{ - const uint8_t whoami = RegisterRead(Register::WHO_AM_I); - - if (whoami != WHOAMI) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; - } - - return PX4_OK; -} - void ICM20602::RunImpl() { const hrt_abstime now = hrt_absolute_time(); @@ -168,20 +170,8 @@ void ICM20602::RunImpl() case STATE::CONFIGURE: if (Configure()) { // if configure succeeded then start reading from FIFO - _state = STATE::FIFO_READ; - - if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; - - // backup schedule as a watchdog timeout - ScheduleDelayed(100_ms); - - } else { - _data_ready_interrupt_enabled = false; - ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); - } - - FIFOReset(); + _state = STATE::CHECK_CONFIGURATION; + ScheduleNow(); } else { // CONFIGURE not complete @@ -198,23 +188,59 @@ void ICM20602::RunImpl() break; + case STATE::CHECK_CONFIGURATION: + if (CheckConfiguration()) { + // if configuration is correct then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + ScheduleNow(); + + } else { + // not fully configured, try again + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + } + + break; + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; uint32_t samples = 0; if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_fifo_read_samples was set as expected - if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { - perf_count(_drdy_missed_perf); + auto flags = px4_enter_critical_section(); + + if ((_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) + && (hrt_elapsed_time(&_drdy_fifo_watermark_timestamp) < (FIFO_SAMPLE_DT * 2))) { + + if (hrt_elapsed_time(&_last_successful_read) > (_fifo_empty_interval_us / 2)) { + // only use if the last transfer wasn't too recent + timestamp_sample = _drdy_fifo_watermark_timestamp; + samples = _fifo_gyro_samples; + } } else { - samples = _fifo_gyro_samples; + perf_count(_drdy_missed_perf); } + px4_leave_critical_section(flags); + // push backup schedule back ScheduleDelayed(_fifo_empty_interval_us * 2); } - if (samples == 0) { + if ((samples == 0) || (timestamp_sample == 0)) { // check current FIFO count const uint16_t fifo_count = FIFOReadCount(); @@ -234,6 +260,9 @@ void ICM20602::RunImpl() FIFOReset(); perf_count(_fifo_overflow_perf); samples = 0; + + } else { + timestamp_sample = now; } } } @@ -241,7 +270,7 @@ void ICM20602::RunImpl() bool success = false; if (samples >= SAMPLES_PER_TRANSFER) { - if (FIFORead(now, samples)) { + if (FIFORead(timestamp_sample, samples)) { success = true; if (_failure_count > 0) { @@ -367,14 +396,8 @@ void ICM20602::ConfigureFIFOWatermark(uint8_t samples) } } -bool ICM20602::Configure() +bool ICM20602::CheckConfiguration() { - // first set and clear all configured register bits - for (const auto ®_cfg : _register_cfg) { - RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); - } - - // now check that all are configured bool success = true; for (const auto ®_cfg : _register_cfg) { @@ -389,6 +412,19 @@ bool ICM20602::Configure() return success; } +bool ICM20602::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = CheckConfiguration(); + + return success; +} + int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) { static_cast(arg)->DataReady(); @@ -400,6 +436,7 @@ void ICM20602::DataReady() uint32_t expected = 0; if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + hrt_store_absolute_time(&_drdy_fifo_watermark_timestamp); ScheduleNow(); } } @@ -509,6 +546,8 @@ bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) const uint8_t valid_samples = math::min(samples, fifo_count_samples); if (valid_samples > 0) { + _last_successful_read = timestamp_sample; + // use raw temperature to first validate FIFO transfer if (ProcessTemperature(buffer.f, valid_samples)) { ProcessGyro(timestamp_sample, buffer.f, valid_samples); diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index a1a2746c47..4c61d3c0ed 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -101,6 +101,7 @@ private: bool Reset(); + bool CheckConfiguration(); bool Configure(); void ConfigureAccel(); void ConfigureGyro(); @@ -143,16 +144,18 @@ private: int _failure_count{0}; px4::atomic _drdy_fifo_read_samples{0}; + hrt_abstime _drdy_fifo_watermark_timestamp{0}; bool _data_ready_interrupt_enabled{false}; + hrt_abstime _last_successful_read{0}; + enum class STATE : uint8_t { RESET, WAIT_FOR_RESET, CONFIGURE, + CHECK_CONFIGURATION, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index 110046544a..f9a384ce69 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -196,6 +196,7 @@ struct DATA { uint8_t GYRO_ZOUT_H; uint8_t GYRO_ZOUT_L; }; +static constexpr uint8_t MAX_SAMPLES{SIZE / sizeof(DATA)}; } } // namespace InvenSense_ICM20602 diff --git a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp index 7a48d456b6..0d5bc4cc43 100644 --- a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp +++ b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions