From 89d4d6e4a9ee4ccb34e0c9d00af89ab68742acdc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 18 Feb 2020 00:10:30 -0500 Subject: [PATCH] invensense mpu9250 improvements - checked register mechanism and simple watchdog - driver checks for errors gradually and can reconfigure itself - respect IMU_GYRO_RATEMAX at the driver level - fixed sensor INT16_MIN and INT16_MAX handling (y & z axis are flipped before publishing) --- .../mpu9250/InvenSense_MPU9250_registers.hpp | 31 +- .../imu/invensense/mpu9250/MPU9250.cpp | 395 +++++++++++++----- .../imu/invensense/mpu9250/MPU9250.hpp | 51 ++- 3 files changed, 348 insertions(+), 129 deletions(-) diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp index 20ba0ca010..bb3fc899e7 100644 --- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp +++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_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); @@ -57,6 +59,9 @@ static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x71; +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, @@ -65,8 +70,6 @@ enum class Register : uint8_t { FIFO_EN = 0x23, - INT_STATUS = 0x3A, - INT_ENABLE = 0x38, TEMP_OUT_H = 0x41, @@ -75,6 +78,8 @@ enum class Register : uint8_t { USER_CTRL = 0x6A, PWR_MGMT_1 = 0x6B, + I2C_IF = 0x70, + FIFO_COUNTH = 0x72, FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, @@ -97,7 +102,7 @@ enum GYRO_CONFIG_BIT : uint8_t { GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 // FCHOICE_B [1:0] - FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz + FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz }; // ACCEL_CONFIG @@ -125,25 +130,25 @@ enum FIFO_EN_BIT : uint8_t { // INT_ENABLE enum INT_ENABLE_BIT : uint8_t { - FIFO_OFLOW_EN = Bit4, - DATA_RDY_INT_EN = Bit0 -}; - -// INT_STATUS -enum INT_STATUS_BIT : uint8_t { - FIFO_OFLOW_INT = Bit4, - DATA_RDY_INT = Bit0, + FIFO_OFLOW_EN = Bit4, + RAW_RDY_EN = Bit0 }; // USER_CTRL enum USER_CTRL_BIT : uint8_t { - FIFO_EN = Bit6, - FIFO_RST = Bit2, + FIFO_EN = Bit6, + I2C_MST_EN = Bit5, + I2C_IF_DIS = Bit4, + + FIFO_RST = Bit2, + I2C_MST_RST = Bit1, + SIG_COND_RST = Bit0, }; // PWR_MGMT_1 enum PWR_MGMT_1_BIT : uint8_t { H_RESET = Bit7, + SLEEP = Bit6, CLKSEL_2 = Bit2, CLKSEL_1 = Bit1, diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 7a9e68ec8b..359ae27f3a 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -36,17 +36,16 @@ #include using namespace time_literals; -using namespace InvenSense_MPU9250; -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{4000}; // 4 kHz accel - -static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} -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 bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1) +{ + return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0); +} MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) : SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), @@ -57,9 +56,6 @@ MPU9250::MPU9250(int bus, uint32_t device, enum Rotation rotation) : set_device_type(DRV_ACC_DEVTYPE_MPU9250); _px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU9250); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU9250); - - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); - _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } MPU9250::~MPU9250() @@ -71,12 +67,34 @@ MPU9250::~MPU9250() } 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); } +void MPU9250::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1 kHz + } + + sample_rate = math::constrain(sample_rate, 250, 2000); // limit 250 - 2000 Hz + + _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 500); // round down to nearest 250 us + _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); + + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); + + _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); +} + int MPU9250::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -96,11 +114,6 @@ bool MPU9250::Init() return false; } - if (!Reset()) { - PX4_ERR("reset failed"); - return false; - } - // allocate DMA capable buffer _dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE); @@ -109,6 +122,11 @@ bool MPU9250::Init() return false; } + if (!Reset()) { + PX4_ERR("reset failed"); + return false; + } + Start(); return true; @@ -117,60 +135,144 @@ bool MPU9250::Init() bool MPU9250::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::H_RESET); - usleep(1000); - // 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(1000); - - // ACCEL_CONFIG: Accel 16 G range - RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G); - _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); - _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + for (int i = 0; i < 100; i++) { + // The reset value is 0x00 for all registers other than the registers below + // Document Number: RM-MPU-9250A-00 Page 9 of 55 + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + return true; + } + } - // GYRO_CONFIG: Gyro 2000 degrees/second - RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS); - _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + return false; +} - // reset done once data is ready - const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::H_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); +void MPU9250::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] + + switch (ACCEL_FS_SEL) { + case ACCEL_FS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _px4_accel.set_range(2 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_4G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192); + _px4_accel.set_range(4 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _px4_accel.set_range(8 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_16G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16 * CONSTANTS_ONE_G); + break; + } +} - return reset_done && clksel_done && data_ready; +void MPU9250::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0] + + switch (GYRO_FS_SEL) { + case GYRO_FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 131.f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case GYRO_FS_SEL_500_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 65.5f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case GYRO_FS_SEL_1000_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.0f)); + break; + + case GYRO_FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + break; + } } void MPU9250::ResetFIFO() { perf_count(_fifo_reset_perf); - // ACCEL_CONFIG2: Accel DLPF disabled for full rate (4 kHz) - RegisterSetBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF); - - // GYRO_CONFIG: Gyro DLPF disabled for full rate (8 kHz) - RegisterClearBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF); + // USER_CTRL: disable FIFO and reset all signal paths + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST, + USER_CTRL_BIT::FIFO_EN); - // FIFO_EN: disable FIFO - RegisterWrite(Register::FIFO_EN, 0); - RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RST); - - // USER_CTRL: reset FIFO then re-enable - RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST); - 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); - - // CONFIG: should ensure that bit 7 of register 0x1A is set to 0 before using FIFO watermark feature - RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE); - RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + _data_ready_count.store(0); // FIFO_EN: enable both gyro and accel - _data_ready_count = 0; RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL); - up_udelay(10); + + // USER_CTRL: re-enable FIFO + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, + USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST); +} + +bool MPU9250::Configure(bool notify) +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!CheckRegister(reg, notify)) { + success = false; + } + } + + return success; +} + +bool MPU9250::CheckRegister(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)) { + if (notify) { + PX4_ERR("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)) { + if (notify) { + PX4_ERR("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 (reg_cfg.reg == Register::ACCEL_CONFIG) { + ConfigureAccel(); + + } else if (reg_cfg.reg == Register::GYRO_CONFIG) { + ConfigureGyro(); + } + + if (notify) { + perf_count(_bad_register_perf); + } + } + + return success; } uint8_t MPU9250::RegisterRead(Register reg) @@ -187,24 +289,30 @@ void MPU9250::RegisterWrite(Register reg, uint8_t value) transfer(cmd, cmd, sizeof(cmd)); } -void MPU9250::RegisterSetBits(Register reg, uint8_t setbits) +void MPU9250::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) { - uint8_t val = RegisterRead(reg); + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = orig_val; - if (!(val & setbits)) { + if (setbits) { val |= setbits; - RegisterWrite(reg, val); } + + if (clearbits) { + val &= ~clearbits; + } + + RegisterWrite(reg, val); } -void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits) +void MPU9250::RegisterSetBits(Register reg, uint8_t setbits) { - uint8_t val = RegisterRead(reg); + RegisterSetAndClearBits(reg, setbits, 0); +} - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); - } +void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits) +{ + RegisterSetAndClearBits(reg, 0, clearbits); } int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -218,81 +326,111 @@ void MPU9250::DataReady() { perf_count(_drdy_interval_perf); - _data_ready_count++; - - if (_data_ready_count >= 8) { - _time_data_ready = hrt_absolute_time(); - - _data_ready_count = 0; - + if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) { // make another measurement ScheduleNow(); + _data_ready_count.store(0); } } void MPU9250::Start() { - Stop(); + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); - ResetFIFO(); + // attempt to configure 3 times + for (int i = 0; i < 3; i++) { + if (Configure(false)) { + break; + } + } // TODO: cleanup horrible DRDY define mess #if defined(GPIO_DRDY_PORTD_PIN15) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, true, false, true, &MPU9250::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); #else + _using_data_ready_interrupt_enabled = false; ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); #endif + + ResetFIFO(); + + // schedule as watchdog + if (_using_data_ready_interrupt_enabled) { + ScheduleDelayed(100_ms); + } } void MPU9250::Stop() { + Reset(); + // TODO: cleanup horrible DRDY define mess #if defined(GPIO_DRDY_PORTD_PIN15) // Disable data ready callback px4_arch_gpiosetevent(GPIO_DRDY_PORTD_PIN15, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); -#else - ScheduleClear(); #endif + + ScheduleClear(); } void MPU9250::Run() { - // 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(); + // use the time now roughly corresponding with the last sample we'll pull from the FIFO + const hrt_abstime timestamp_sample = hrt_absolute_time(); // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { - return; + perf_count(_bad_transfer_perf); + } + + if (_using_data_ready_interrupt_enabled) { + // re-schedule as watchdog + ScheduleDelayed(100_ms); } - 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 + // check registers + if (hrt_elapsed_time(&_last_config_check) > 100_ms) { + _checked_register = (_checked_register + 1) % size_register_cfg; + + if (CheckRegister(_register_cfg[_checked_register])) { + // delay next register check if current succeeded + _last_config_check = hrt_absolute_time(); + + } else { + // if register check failed reconfigure all + Configure(); + ResetFIFO(); + return; + } + } + + const uint16_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); + const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 if (samples < 2) { perf_count(_fifo_empty_perf); return; - } else if (samples > 16) { - // not technically an overflow, but more samples than we expected + } else if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish perf_count(_fifo_overflow_perf); ResetFIFO(); + return; } // Transfer data struct TransferBuffer { uint8_t cmd; - FIFO::DATA f[16]; // max 16 samples + FIFO::DATA f[FIFO_MAX_SAMPLES]; }; - static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding + // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES * sizeof(FIFO::DATA))); TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); @@ -303,43 +441,73 @@ void MPU9250::Run() if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); return; } perf_end(_transfer_perf); + PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; - accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES; + accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + + // accel data is doubled in FIFO, but might be shifted + int accel_first_sample = 0; + + if (samples >= 3) { + if (fifo_accel_equal(report->f[0], report->f[1])) { + // [A0, A1, A2, A3] + // A0==A1, A2==A3 + accel_first_sample = 1; + + } else if (fifo_accel_equal(report->f[1], report->f[2])) { + // [A0, A1, A2, A3] + // A0, A1==A2, A3 + accel_first_sample = 0; + + } else { + perf_count(_bad_transfer_perf); + return; + } + } + + int accel_samples = 0; + + for (int i = accel_first_sample; i < samples; i = i + 2) { + const FIFO::DATA &fifo_sample = report->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 (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; + PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; gyro.samples = samples; - gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; - - int accel_samples = 0; + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; for (int i = 0; i < samples; i++) { const FIFO::DATA &fifo_sample = report->f[i]; - // accel data is doubled - if (i % 2) { - // coordinate convention (x forward, y right, z down) - accel.x[accel_samples] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); - accel.y[accel_samples] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); - accel.z[accel_samples] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_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); - accel_samples++; - } - - // 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); + // sensor's frame is +x forward, +y left, +z up, flip y & z to publish right handed (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; } - accel.samples = accel_samples; - // Temperature if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { // read current temperature @@ -351,11 +519,7 @@ void MPU9250::Run() } 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; + const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET; _px4_accel.set_temperature(TEMP_degC); _px4_gyro.set_temperature(TEMP_degC); @@ -367,7 +531,12 @@ void MPU9250::Run() void MPU9250::PrintInfo() { + 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); diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 408962bc97..61cfafc0bb 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -48,9 +48,10 @@ #include #include #include +#include #include -using InvenSense_MPU9250::Register; +using namespace InvenSense_MPU9250; class MPU9250 : public device::SPI, public px4::ScheduledWorkItem { @@ -65,6 +66,13 @@ public: void PrintInfo(); private: + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + int probe() override; static int DataReadyInterruptCallback(int irq, void *context, void *arg); @@ -72,8 +80,16 @@ private: void Run() override; + void ConfigureSampleRate(int sample_rate); + bool CheckRegister(const register_config_t ®_cfg, bool notify = true); + bool Configure(bool notify = true); + + void ConfigureAccel(); + void ConfigureGyro(); + uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); void RegisterSetBits(Register reg, uint8_t setbits); void RegisterClearBits(Register reg, uint8_t clearbits); @@ -85,12 +101,41 @@ private: PX4Gyroscope _px4_gyro; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + 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 _last_config_check{0}; hrt_abstime _time_last_temperature_update{0}; - int _data_ready_count{0}; + + px4::atomic _data_ready_count{0}; + + uint8_t _checked_register{0}; + + bool _using_data_ready_interrupt_enabled{false}; + + // Sensor Configuration + static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro + static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel + static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + + uint16_t _fifo_empty_interval_us{1000}; // 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))}; + + static constexpr uint8_t size_register_cfg{11}; + 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::H_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, + { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF }, + { Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, + { Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 }, + { Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN | INT_ENABLE_BIT::RAW_RDY_EN } + }; };