diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp index 89134d9c18..f287b2d13e 100644 --- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp +++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp @@ -55,7 +55,7 @@ static constexpr uint8_t Bit7 = (1 << 7); namespace InvenSense_MPU6000 { static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000; -static constexpr uint32_t SPI_SPEED_SENSOR = 10 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers +static constexpr uint32_t SPI_SPEED_SENSOR = 5 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x68; diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 187d07ec45..95e9cf5572 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -49,7 +49,7 @@ MPU6000::MPU6000(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) { 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()); @@ -62,7 +62,7 @@ MPU6000::~MPU6000() 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); } int MPU6000::init() @@ -96,15 +96,14 @@ void MPU6000::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 MPU6000::probe() @@ -128,8 +127,7 @@ void MPU6000::RunImpl() // PWR_MGMT_1: Device Reset RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); _reset_timestamp = now; - _consecutive_failures = 0; - _total_failures = 0; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(100_ms); // Wait 100ms (Document Number: RM-MPU-6000A-00 Page 41 of 46) break; @@ -142,10 +140,10 @@ void MPU6000::RunImpl() && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { // 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::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); - RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RESET, USER_CTRL_BIT::I2C_IF_DIS); + RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RESET | USER_CTRL_BIT::I2C_IF_DIS); // if reset succeeded then configure _state = STATE::CONFIGURE; @@ -194,7 +192,7 @@ void MPU6000::RunImpl() PX4_DEBUG("Configure failed, retrying"); } - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } break; @@ -202,8 +200,8 @@ void MPU6000::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_gyro_samples) { + perf_count(_drdy_missed_perf); } // push backup schedule back @@ -233,23 +231,25 @@ void MPU6000::RunImpl() } else if (samples >= 1) { if (FIFORead(now, samples)) { 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; @@ -375,12 +375,12 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU6000::DataReady() { - const uint8_t count = _drdy_count.fetch_add(1) + 1; - - uint8_t expected = 0; + uint32_t expected = 0; // 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); ScheduleNow(); } diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 6d50896299..86eb5b9ecc 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -133,19 +133,18 @@ private: 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_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 _last_config_check_timestamp{0}; hrt_abstime _temperature_update_timestamp{0}; - unsigned _consecutive_failures{0}; - unsigned _total_failures{0}; + int _failure_count{0}; FIFO::DATA _fifo_sample_last_new_accel{}; - int _fifo_accel_samples_count{0}; + uint32_t _fifo_accel_samples_count{0}; - px4::atomic _drdy_fifo_read_samples{0}; - px4::atomic _drdy_count{0}; + px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -158,7 +157,7 @@ private: STATE _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval - uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{7}; @@ -170,6 +169,6 @@ private: { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 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::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 }, }; };