diff --git a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt index 53d72cf5be..65af48a8e8 100644 --- a/src/drivers/imu/invensense/mpu6000/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu6000/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( MPU6000.cpp MPU6000.hpp mpu6000_main.cpp + InvenSense_MPU6000_registers.hpp DEPENDS drivers_accelerometer drivers_gyroscope diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp index 1ccde66675..45a8427717 100644 --- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp +++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp @@ -40,6 +40,8 @@ #pragma once +#include + // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); static constexpr uint8_t Bit1 = (1 << 1); @@ -52,25 +54,33 @@ static constexpr uint8_t Bit7 = (1 << 7); namespace InvenSense_MPU6000 { -static constexpr uint32_t SPI_SPEED = 20 * 1000 * 1000; +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 uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x68; +static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C +static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C + enum class Register : uint8_t { CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, FIFO_EN = 0x23, + I2C_MST_CTRL = 0x24, + + INT_PIN_CFG = 0x37, + INT_ENABLE = 0x38, INT_STATUS = 0x3A, - INT_ENABLE = 0x38, - TEMP_OUT_H = 0x41, TEMP_OUT_L = 0x42, + SIGNAL_PATH_RESET = 0x68, + USER_CTRL = 0x6A, PWR_MGMT_1 = 0x6B, @@ -112,6 +122,14 @@ enum FIFO_EN_BIT : uint8_t { ACCEL_FIFO_EN = Bit3, }; +// INT_PIN_CFG +enum INT_PIN_CFG_BIT : uint8_t { + INT_LEVEL = Bit7, + + INT_RD_CLEAR = Bit4, + +}; + // INT_ENABLE enum INT_ENABLE_BIT : uint8_t { FIFO_OFLOW_EN = Bit4, @@ -124,22 +142,34 @@ enum INT_STATUS_BIT : uint8_t { DATA_RDY_INT = Bit0, }; +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + GYRO_RESET = Bit2, + ACCEL_RESET = Bit1, + TEMP_RESET = Bit0, +}; + // USER_CTRL enum USER_CTRL_BIT : uint8_t { - FIFO_EN = Bit6, - FIFO_RESET = Bit2, + FIFO_EN = Bit6, + I2C_MST_EN = Bit5, + I2C_IF_DIS = Bit4, + + FIFO_RESET = Bit2, + + SIG_COND_RESET = Bit0, }; // PWR_MGMT_1 enum PWR_MGMT_1_BIT : uint8_t { DEVICE_RESET = Bit7, + SLEEP = Bit6, - CLKSEL_2 = Bit2, - CLKSEL_1 = Bit1, - CLKSEL_0 = Bit0, + CLKSEL_2 = Bit2, + CLKSEL_1 = Bit1, + CLKSEL_0 = Bit0, }; - namespace FIFO { static constexpr size_t SIZE = 1024; diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index 1585417891..d700c60188 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -34,43 +34,83 @@ #include "MPU6000.hpp" using namespace time_literals; -using namespace InvenSense_MPU6000; -static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } - -static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro -static constexpr uint32_t ACCEL_RATE{1000}; // 1 kHz accel - -static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval - -static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)}; -static constexpr uint32_t FIFO_ACCEL_SAMPLES{FIFO_INTERVAL / (1000000 / ACCEL_RATE)}; +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} MPU6000::MPU6000(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : SPI(MODULE_NAME, nullptr, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) + _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) { set_device_type(DRV_IMU_DEVTYPE_MPU6000); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_MPU6000); _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_MPU6000); - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); - _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); } MPU6000::~MPU6000() { perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); perf_free(_fifo_empty_perf); perf_free(_fifo_overflow_perf); perf_free(_fifo_reset_perf); perf_free(_drdy_interval_perf); } +int MPU6000::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool MPU6000::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void MPU6000::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void MPU6000::print_status() +{ + I2CSPIDriverBase::print_status(); + PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, + static_cast(1000000 / _fifo_empty_interval_us)); + + perf_print_counter(_transfer_perf); + 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); + + _px4_accel.print_status(); + _px4_gyro.print_status(); +} + int MPU6000::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -83,126 +123,232 @@ int MPU6000::probe() return PX4_OK; } -void MPU6000::exit_and_cleanup() +void MPU6000::RunImpl() { - if (_drdy_gpio != 0) { - // Disable data ready callback - px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); - } + switch (_state) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + _reset_timestamp = hrt_absolute_time(); + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(100_ms); + break; - I2CSPIDriverBase::exit_and_cleanup(); -} + case STATE::WAIT_FOR_RESET: -int MPU6000::init() -{ - int ret = SPI::init(); + // The reset value is 0x00 for all registers other than the registers below + // Document Number: RM-MPU-6000A-00 Page 8 of 46 + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { - if (ret != PX4_OK) { - DEVICE_DEBUG("SPI::init failed (%i)", ret); - return ret; - } + // SIGNAL_PATH_RESET: ensure the reset is performed properly + RegisterWrite(Register::SIGNAL_PATH_RESET, + SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); - if (!Reset()) { - DEVICE_DEBUG("reset failed"); - return PX4_ERROR; - } + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(100_ms); - Start(); + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); - return PX4_OK; -} + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } -bool MPU6000::Reset() -{ - // PWR_MGMT_1: Device Reset - // CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. - RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); - usleep(2000); + break; - // PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. - RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); - usleep(100); + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; - // ACCEL_CONFIG: Accel 16 G range - RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::AFS_SEL_16G); - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); - _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; - // GYRO_CONFIG: Gyro 2000 degrees/second - RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS); - _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); - // reset done once data is ready - const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::DEVICE_RESET); - const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0); - const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT); + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } - return reset_done && clksel_done && data_ready; -} + FIFOReset(); -void MPU6000::ResetFIFO() -{ - perf_count(_fifo_reset_perf); + } else { + PX4_DEBUG("Configure failed, retrying"); + // try again in 10 ms + ScheduleDelayed(10_ms); + } - // FIFO_EN: disable FIFO - RegisterWrite(Register::FIFO_EN, 0); - RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RESET); + break; - // USER_CTRL: reset FIFO then re-enable - RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET); - up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock - RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN); + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; - // CONFIG: - RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF); + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + // re-schedule as watchdog timeout + ScheduleDelayed(10_ms); - // FIFO_EN: enable both gyro and accel - _data_ready_count = 0; - RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | - FIFO_EN_BIT::ACCEL_FIFO_EN); - up_udelay(10); -} + timestamp_sample = _fifo_watermark_interrupt_timestamp; -uint8_t MPU6000::RegisterRead(Register reg) -{ - uint8_t cmd[2] {}; - cmd[0] = static_cast(reg) | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); - return cmd[1]; -} + } else { + // use the time now roughly corresponding with the last sample we'll pull from the FIFO + timestamp_sample = hrt_absolute_time(); + } -void MPU6000::RegisterWrite(Register reg, uint8_t value) -{ - uint8_t cmd[2] { (uint8_t)reg, value }; - transfer(cmd, cmd, sizeof(cmd)); -} + const uint16_t fifo_count = FIFOReadCount(); + const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * + SAMPLES_PER_TRANSFER; // round down to nearest -void MPU6000::RegisterSetBits(Register reg, uint8_t setbits) -{ - uint8_t val = RegisterRead(reg); + bool failure = false; - if (!(val & setbits)) { - val |= setbits; - RegisterWrite(reg, val); + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + failure = true; + FIFOReset(); + + } else if (samples >= SAMPLES_PER_TRANSFER) { + // require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer) + if (!FIFORead(timestamp_sample, samples)) { + failure = true; + _px4_accel.increase_error_count(); + _px4_gyro.increase_error_count(); + } + + } else if (samples == 0) { + failure = true; + perf_count(_fifo_empty_perf); + } + + if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + // check registers incrementally + if (RegisterCheck(_register_cfg[_checked_register], true)) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reconfigure + PX4_DEBUG("Health check failed, reconfiguring"); + _state = STATE::CONFIGURE; + ScheduleNow(); + } + + } else { + // periodically update temperature (1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = timestamp_sample; + } + } + } + + break; } } -void MPU6000::RegisterClearBits(Register reg, uint8_t clearbits) +void MPU6000::ConfigureAccel() { - uint8_t val = RegisterRead(reg); + const uint8_t AFS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] AFS_SEL[1:0] - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); + switch (AFS_SEL) { + case AFS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _px4_accel.set_range(2 * CONSTANTS_ONE_G); + break; + + case AFS_SEL_4G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192); + _px4_accel.set_range(4 * CONSTANTS_ONE_G); + break; + + case AFS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _px4_accel.set_range(8 * CONSTANTS_ONE_G); + break; + + case AFS_SEL_16G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16 * CONSTANTS_ONE_G); + break; } } +void MPU6000::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] + + switch (GYRO_FS_SEL) { + case FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.f / 131.f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case FS_SEL_500_DPS: + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case FS_SEL_1000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.f)); + break; + + case FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.f)); + break; + } +} + +void MPU6000::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1 kHz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = SAMPLES_PER_TRANSFER * 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); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); + + _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); +} + +bool MPU6000::Configure() +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + ConfigureAccel(); + ConfigureGyro(); + + return success; +} + int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg) { - MPU6000 *dev = reinterpret_cast(arg); - dev->DataReady(); + static_cast(arg)->DataReady(); return 0; } @@ -210,143 +356,226 @@ void MPU6000::DataReady() { perf_count(_drdy_interval_perf); - _data_ready_count++; - - if (_data_ready_count >= 8) { - _time_data_ready = hrt_absolute_time(); - - _data_ready_count = 0; - - // make another measurement + if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) { + _data_ready_count.store(0); + _fifo_watermark_interrupt_timestamp = hrt_absolute_time(); + _fifo_read_samples.store(_fifo_gyro_samples); ScheduleNow(); } } -void MPU6000::Start() +bool MPU6000::DataReadyInterruptConfigure() { - ResetFIFO(); - if (_drdy_gpio == 0) { - ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); - - } else { - // Setup data ready on rising edge - px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &MPU6000::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); + return false; } + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &MPU6000::DataReadyInterruptCallback, this) == 0; } -void MPU6000::RunImpl() +bool MPU6000::DataReadyInterruptDisable() { - // use timestamp from the data ready interrupt if available, - // otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO - const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready : - hrt_absolute_time(); + if (_drdy_gpio == 0) { + return false; + } + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool MPU6000::RegisterCheck(const register_config_t ®_cfg, bool notify) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + if (!success) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + + if (notify) { + perf_count(_bad_register_perf); + _px4_accel.increase_error_count(); + _px4_gyro.increase_error_count(); + } + } + + return success; +} + +uint8_t MPU6000::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + set_frequency(SPI_SPEED); // low speed for regular registers + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void MPU6000::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + set_frequency(SPI_SPEED); // low speed for regular registers + transfer(cmd, cmd, sizeof(cmd)); +} + +void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = orig_val; + + if (setbits) { + val |= setbits; + } + + if (clearbits) { + val &= ~clearbits; + } + + RegisterWrite(reg, val); +} + +uint16_t MPU6000::FIFOReadCount() +{ // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; + set_frequency(SPI_SPEED_SENSOR); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { - return; + perf_count(_bad_transfer_perf); + return 0; } - const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); - const int samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} - if (samples < 1) { - perf_count(_fifo_empty_perf); - return; +bool MPU6000::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) +{ + perf_begin(_transfer_perf); - } else if (samples < 8) { - // don't transfer fewer than 8 samples (1 accel + 8 gyro) - return; - - } else if (samples > 16) { - // not technically an overflow, but more samples than we expected - perf_count(_fifo_overflow_perf); - ResetFIFO(); - return; - } - - // Transfer data FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); - - perf_begin(_transfer_perf); + set_frequency(SPI_SPEED_SENSOR); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { perf_end(_transfer_perf); - return; + perf_count(_bad_transfer_perf); + return false; } perf_end(_transfer_perf); + ProcessGyro(timestamp_sample, buffer, samples); + return ProcessAccel(timestamp_sample, buffer, samples); +} + +void MPU6000::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_EN: disable FIFO + RegisterWrite(Register::FIFO_EN, 0); + + // USER_CTRL: reset FIFO + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET, USER_CTRL_BIT::FIFO_EN); + + // reset while FIFO is disabled + _data_ready_count.store(0); + _fifo_watermark_interrupt_timestamp = 0; + _fifo_read_samples.store(0); + + // FIFO_EN: enable both gyro and accel + // USER_CTRL: re-enable FIFO + for (const auto &r : _register_cfg) { + if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) { + RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); + } + } +} + +bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) +{ PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; - accel.samples = samples / 8; - accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES; + accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + bool bad_data = false; + + // FIFO contains 8 duplicated accel samples per gyro sample + int accel_samples = 0; + + for (int i = 0; i < samples; i = i + 8) { + const FIFO::DATA &fifo_sample = buffer.f[i]; + int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); + int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); + int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[accel_samples] = accel_x; + accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel_samples++; + } + + accel.samples = accel_samples; + + _px4_accel.updateFIFO(accel); + + return !bad_data; +} + +void MPU6000::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) +{ PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; - gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; - - // accel data is duplicated 8 times - for (int i = 0; i < accel.samples; i++) { - const FIFO::DATA &fifo_sample = buffer.f[i]; - - // coordinate convention (x forward, y right, z down) - accel.x[i] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); - accel.y[i] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); - accel.z[i] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); - } + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = buffer.f[i]; - // coordinate convention (x forward, y right, z down) - gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); - gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); - gyro.z[i] = -combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); - } + const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); + const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); + const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); - - // Temperature - if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { - // read current temperature - uint8_t temperature_buf[3] {}; - temperature_buf[0] = static_cast(Register::TEMP_OUT_H) | DIR_READ; - - if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { - return; - } - - const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); - - static constexpr float RoomTemp_Offset = 25.0f; // Room Temperature Offset 25°C - static constexpr float Temp_Sensitivity = 326.8f; // Sensitivity 326.8 LSB/°C - - const float TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 25.0f; - - _px4_accel.set_temperature(TEMP_degC); - _px4_gyro.set_temperature(TEMP_degC); + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; } _px4_gyro.updateFIFO(gyro); - _px4_accel.updateFIFO(accel); } -void MPU6000::print_status() +void MPU6000::UpdateTemperature() { - I2CSPIDriverBase::print_status(); - perf_print_counter(_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); + // read current temperature + uint8_t temperature_buf[3] {}; + temperature_buf[0] = static_cast(Register::TEMP_OUT_H) | DIR_READ; + set_frequency(SPI_SPEED_SENSOR); - _px4_accel.print_status(); - _px4_gyro.print_status(); + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); + const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + } } diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 91f08101b1..1373a0beca 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -48,9 +48,10 @@ #include #include #include +#include #include -using InvenSense_MPU6000::Register; +using namespace InvenSense_MPU6000; class MPU6000 : public device::SPI, public I2CSPIDriver { @@ -75,6 +76,13 @@ protected: void custom_method(const BusCLIArguments &cli) override; void exit_and_cleanup() override; private: + // Sensor Configuration + static constexpr float FIFO_SAMPLE_DT{125.f}; + static constexpr uint32_t SAMPLES_PER_TRANSFER{8}; // ensure at least 1 new accel sample per transfer + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro + static constexpr float ACCEL_RATE{GYRO_RATE / 8.f}; // 1 kHz accel + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; // Transfer data struct FIFOTransferBuffer { @@ -82,19 +90,41 @@ private: FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; int probe() override; + bool Configure(); + void ConfigureAccel(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); - void RegisterSetBits(Register reg, uint8_t setbits); - void RegisterClearBits(Register reg, uint8_t clearbits); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } - void ResetFIFO(); + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); + void FIFOReset(); + + bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); + void UpdateTemperature(); const spi_drdy_gpio_t _drdy_gpio; @@ -102,12 +132,45 @@ private: PX4Gyroscope _px4_gyro; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; - 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{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + 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{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")}; - hrt_abstime _time_data_ready{0}; - hrt_abstime _time_last_temperature_update{0}; - int _data_ready_count{0}; + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _fifo_watermark_interrupt_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + + px4::atomic _data_ready_count{0}; + px4::atomic _fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + }; + + STATE _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{7}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::AFS_SEL_16G, 0 }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, 0 }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::I2C_MST_EN}, + { Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN }, + { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 }, + { Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 } + }; };