diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake index dcc9958f76..12a346b854 100644 --- a/boards/px4/fmu-v4/cannode.cmake +++ b/boards/px4/fmu-v4/cannode.cmake @@ -59,7 +59,7 @@ px4_add_board( MODULES #ekf2 #load_mon - #sensors + sensors #temperature_compensation SYSTEMCMDS #bl_update diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 5d5eb340b0..96f6ee161d 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -30,6 +30,7 @@ px4_add_board( imu/adis16497 imu/invensense/icm20602 imu/invensense/icm20608-g + #imu/invensense/mpu9250 imu/mpu6000 imu/mpu9250 irlock diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg index ee6f292a08..b9c20048d1 100644 --- a/msg/sensor_accel_status.msg +++ b/msg/sensor_accel_status.msg @@ -10,7 +10,7 @@ uint8 rotation uint32[3] clipping # clipping per axis -uint16 measure_rate +uint16 measure_rate_hz float32 full_scale_range diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 677d5dba8d..24cc131806 100644 --- a/msg/sensor_gyro_fifo.msg +++ b/msg/sensor_gyro_fifo.msg @@ -8,6 +8,6 @@ float32 scale uint8 samples # number of valid samples -int16[16] x # angular velocity in the NED X board axis in rad/s -int16[16] y # angular velocity in the NED Y board axis in rad/s -int16[16] z # angular velocity in the NED Z board axis in rad/s +int16[32] x # angular velocity in the NED X board axis in rad/s +int16[32] y # angular velocity in the NED Y board axis in rad/s +int16[32] z # angular velocity in the NED Z board axis in rad/s diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg index ff027256f5..5258c260f1 100644 --- a/msg/sensor_gyro_status.msg +++ b/msg/sensor_gyro_status.msg @@ -10,7 +10,7 @@ uint8 rotation uint32[3] clipping # clipping per axis -uint16 measure_rate +uint16 measure_rate_hz float32 full_scale_range diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index aa2575f32f..49e7c9c890 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,13 +50,13 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI0{"wq:SPI0", 1900, -1}; -static constexpr wq_config_t SPI1{"wq:SPI1", 1900, -2}; -static constexpr wq_config_t SPI2{"wq:SPI2", 1900, -3}; -static constexpr wq_config_t SPI3{"wq:SPI3", 1900, -4}; -static constexpr wq_config_t SPI4{"wq:SPI4", 1900, -5}; -static constexpr wq_config_t SPI5{"wq:SPI5", 1900, -6}; -static constexpr wq_config_t SPI6{"wq:SPI6", 1900, -7}; +static constexpr wq_config_t SPI0{"wq:SPI0", 2000, -1}; +static constexpr wq_config_t SPI1{"wq:SPI1", 2000, -2}; +static constexpr wq_config_t SPI2{"wq:SPI2", 2000, -3}; +static constexpr wq_config_t SPI3{"wq:SPI3", 2000, -4}; +static constexpr wq_config_t SPI4{"wq:SPI4", 2000, -5}; +static constexpr wq_config_t SPI5{"wq:SPI5", 2000, -6}; +static constexpr wq_config_t SPI6{"wq:SPI6", 2000, -7}; static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8}; static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9}; diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 25350523e2..e2117cc871 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -36,17 +36,16 @@ #include using namespace time_literals; -using namespace InvenSense_ICM20602; -static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } +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 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); +} ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), @@ -57,9 +56,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : set_device_type(DRV_ACC_DEVTYPE_ICM20602); _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602); - - _px4_accel.set_update_rate(1000000 / FIFO_INTERVAL); - _px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL); } ICM20602::~ICM20602() @@ -71,12 +67,46 @@ ICM20602::~ICM20602() } 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 ICM20602::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); + + // FIFO watermark threshold in number of bytes + const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_WM_TH1) { + r.set_bits = (fifo_watermark_threshold >> 8) & 0b00000011; + + } else if (r.reg == Register::FIFO_WM_TH2) { + r.set_bits = fifo_watermark_threshold & 0xFF; + } + } +} + int ICM20602::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -96,11 +126,6 @@ bool ICM20602::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 +134,11 @@ bool ICM20602::Init() return false; } + if (!Reset()) { + PX4_ERR("reset failed"); + return false; + } + Start(); return true; @@ -117,64 +147,142 @@ bool ICM20602::Init() bool ICM20602::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(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); + for (int i = 0; i < 100; i++) { + // The reset value is 0x00 for all registers other than the registers below + // Document Number: DS-000176 Page 31 of 57 + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::PWR_MGMT_1) == 0x41) + && (RegisterRead(Register::CONFIG) == 0x80)) { + return true; + } + } - // 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); + return false; +} - // 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)); +void ICM20602::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] - // 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); + 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; - return reset_done && clksel_done && data_ready; + 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; + } +} + +void ICM20602::ConfigureGyro() +{ + const uint8_t FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] FS_SEL[1:0] + + switch (FS_SEL) { + case FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 131.f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case FS_SEL_500_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 65.5f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case FS_SEL_1000_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.0f)); + break; + + case FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + break; + } } void ICM20602::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); - - // 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: User should ensure that bit 7 of register 0x1A (Register::CONFIG) is set to 0 before using watermark feature - RegisterClearBits(Register::CONFIG, Bit7); - RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); - - // FIFO Watermark - static constexpr uint8_t fifo_watermark = 8 * sizeof(FIFO::DATA); - static_assert(fifo_watermark < UINT8_MAX); - RegisterWrite(Register::FIFO_WM_TH1, 0); - RegisterWrite(Register::FIFO_WM_TH2, fifo_watermark); + // 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: enable both gyro and accel - RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); - up_udelay(10); + RegisterSetBits(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); + + // 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 ICM20602::Configure(bool notify) +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!CheckRegister(reg, notify)) { + success = false; + } + } + + return success; +} + +bool ICM20602::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 ICM20602::RegisterRead(Register reg) @@ -191,24 +299,30 @@ void ICM20602::RegisterWrite(Register reg, uint8_t value) transfer(cmd, cmd, sizeof(cmd)); } +void ICM20602::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); +} + void ICM20602::RegisterSetBits(Register reg, uint8_t setbits) { - uint8_t val = RegisterRead(reg); - - if (!(val & setbits)) { - val |= setbits; - RegisterWrite(reg, val); - } + RegisterSetAndClearBits(reg, setbits, 0); } void ICM20602::RegisterClearBits(Register reg, uint8_t clearbits) { - uint8_t val = RegisterRead(reg); - - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); - } + RegisterSetAndClearBits(reg, 0, clearbits); } int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -222,108 +336,136 @@ void ICM20602::DataReady() { perf_count(_drdy_interval_perf); - _time_data_ready = hrt_absolute_time(); - // make another measurement ScheduleNow(); } void ICM20602::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_PORTC_PIN14) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY4_ICM20602) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_DRDY_ICM_2060X) + _using_data_ready_interrupt_enabled = true; // Setup data ready on rising edge px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this); - RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #else - ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); + _using_data_ready_interrupt_enabled = false; + ScheduleOnInterval(FIFO_EMPTY_INTERVAL_US, FIFO_EMPTY_INTERVAL_US); #endif + + ResetFIFO(); + + // schedule as watchdog + if (_using_data_ready_interrupt_enabled) { + ScheduleDelayed(100_ms); + } } void ICM20602::Stop() { + Reset(); + // TODO: cleanup horrible DRDY define mess #if defined(GPIO_DRDY_PORTC_PIN14) // Disable data ready callback px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY4_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_SPI1_DRDY1_ICM20602) // Disable data ready callback px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); #elif defined(GPIO_DRDY_ICM_2060X) // Disable data ready callback px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); - RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN); -#else - ScheduleClear(); #endif + + ScheduleClear(); } void ICM20602::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; - //const hrt_abstime timestamp_fifo_check = hrt_absolute_time(); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { - return; + perf_count(_bad_transfer_perf); } - 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 + if (_using_data_ready_interrupt_enabled) { + // re-schedule as watchdog + ScheduleDelayed(100_ms); + } + + // 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); @@ -334,45 +476,76 @@ void ICM20602::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; + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; - int accel_samples = 0; - int16_t temperature[samples] {}; + int16_t temperature[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); - - accel_samples++; - } - temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L); - // 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); - accel.samples = accel_samples; + // 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; + } // Temperature int32_t temperature_sum{0}; @@ -381,17 +554,18 @@ void ICM20602::Run() temperature_sum += t; } - const int16_t temperature_avg = temperature_sum / samples; + const float temperature_avg = temperature_sum / samples; for (auto t : temperature) { // temperature changing wildly is an indication of a transfer error - if (abs(t - temperature_avg) > 1000) { + if (fabsf(t - temperature_avg) > 1000) { + perf_count(_bad_transfer_perf); return; } } // use average temperature reading - const float temperature_C = temperature_avg / 326.8f + 25.0f; // 326.8 LSB/C + const float temperature_C = temperature_avg / TEMPERATURE_SENSITIVITY + ROOM_TEMPERATURE_OFFSET; _px4_accel.set_temperature(temperature_C); _px4_gyro.set_temperature(temperature_C); @@ -402,7 +576,12 @@ void ICM20602::Run() void ICM20602::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/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index f0fbc98111..5ed72d95f9 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -50,7 +50,7 @@ #include #include -using InvenSense_ICM20602::Register; +using namespace InvenSense_ICM20602; class ICM20602 : public device::SPI, public px4::ScheduledWorkItem { @@ -65,6 +65,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 +79,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,10 +100,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}; + + 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::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS, 0 }, + { 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::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::FIFO_WM_TH1, 0, 0 }, // FIFO_WM_TH[9:8] + { Register::FIFO_WM_TH2, 0, 0 }, // FIFO_WM_TH[7:0] + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, + { Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, 0 }, + { Register::INT_ENABLE, INT_ENABLE_BIT::FIFO_OFLOW_EN, INT_ENABLE_BIT::DATA_RDY_INT_EN } + }; }; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index dd227d53a2..9391c5c1df 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_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); @@ -58,6 +60,9 @@ static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x12; +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, @@ -66,10 +71,7 @@ enum class Register : uint8_t { FIFO_EN = 0x23, - INT_STATUS = 0x3A, - INT_ENABLE = 0x38, - FIFO_WM_INT_STATUS = 0x39, TEMP_OUT_H = 0x41, TEMP_OUT_L = 0x42, @@ -80,6 +82,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, @@ -102,7 +106,7 @@ enum GYRO_CONFIG_BIT : uint8_t { 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 @@ -131,26 +135,28 @@ enum INT_ENABLE_BIT : uint8_t { DATA_RDY_INT_EN = Bit0 }; -// INT_STATUS -enum INT_STATUS_BIT : uint8_t { - FIFO_OFLOW_INT = Bit4, - DATA_RDY_INT = Bit0, -}; - // USER_CTRL enum USER_CTRL_BIT : uint8_t { - FIFO_EN = Bit6, - FIFO_RST = Bit2, + FIFO_EN = Bit6, + FIFO_RST = Bit2, + SIG_COND_RST = 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, }; +// I2C_IF +enum I2C_IF_BIT : uint8_t { + I2C_IF_DIS = Bit6, // 1 – Disable I2C Slave module and put the serial interface in SPI mode only. +}; + namespace FIFO { diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 05de4474d4..bf6345c7bb 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len) return sum; } -static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) +static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) { unsigned clip_count = 0; for (int n = 0; n < len; n++) { - if (abs(samples[n]) > clip_limit) { + if (abs(samples[n]) >= clip_limit) { clip_count++; } } @@ -120,7 +120,10 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) void PX4Accelerometer::set_update_rate(uint16_t rate) { + _update_rate = rate; const uint32_t update_interval = 1000000 / rate; + + // TODO: set this intelligently _integrator_reset_samples = 4000 / update_interval; } @@ -144,7 +147,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl // publish raw data immediately { - sensor_accel_s report{}; + sensor_accel_s report; report.timestamp_sample = timestamp_sample; report.device_id = _device_id; @@ -166,7 +169,7 @@ void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, fl if (_integrator.put(timestamp_sample, val_calibrated, delta_velocity, integral_dt)) { // fill sensor_accel_integrated and publish - sensor_accel_integrated_s report{}; + sensor_accel_integrated_s report; report.timestamp_sample = timestamp_sample; report.error_count = _error_count; @@ -208,7 +211,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) // Apply range scale and the calibrating offset/scale const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)}; - sensor_accel_s report{}; + sensor_accel_s report; report.timestamp_sample = sample.timestamp_sample; report.device_id = _device_id; @@ -269,7 +272,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample) delta_velocity *= 1e-6f * dt; // fill sensor_accel_integrated and publish - sensor_accel_integrated_s report{}; + sensor_accel_integrated_s report; report.timestamp_sample = sample.timestamp_sample; report.error_count = _error_count; @@ -316,13 +319,13 @@ void PX4Accelerometer::PublishStatus() { // publish sensor status if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) { - sensor_accel_status_s status{}; + sensor_accel_status_s status; status.device_id = _device_id; status.error_count = _error_count; status.full_scale_range = _range; status.rotation = _rotation; - status.measure_rate = _update_rate; + status.measure_rate_hz = _update_rate; status.temperature = _temperature; status.vibration_metric = _vibration_metric; status.clipping[0] = _clipping[0]; @@ -347,8 +350,8 @@ void PX4Accelerometer::ResetIntegrator() void PX4Accelerometer::UpdateClipLimit() { - // 95% of potential max - _clip_limit = (_range / _scale) * 0.95f; + // 99.9% of potential max + _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); } void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 098ddc6042..3bcd4fbeb4 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -50,12 +50,12 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len) return sum; } -static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) +static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len) { unsigned clip_count = 0; for (int n = 0; n < len; n++) { - if (abs(samples[n]) > clip_limit) { + if (abs(samples[n]) >= clip_limit) { clip_count++; } } @@ -65,6 +65,7 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) : CDev(nullptr), + ModuleParams(nullptr), _sensor_pub{ORB_ID(sensor_gyro), priority}, _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority}, _sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority}, @@ -74,6 +75,8 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r _rotation_dcm{get_rot_matrix(rotation)} { _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); + + updateParams(); } PX4Gyroscope::~PX4Gyroscope() @@ -119,7 +122,10 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) void PX4Gyroscope::set_update_rate(uint16_t rate) { + _update_rate = rate; const uint32_t update_interval = 1000000 / rate; + + // TODO: set this intelligently _integrator_reset_samples = 4000 / update_interval; } @@ -143,7 +149,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float // publish raw data immediately { - sensor_gyro_s report{}; + sensor_gyro_s report; report.timestamp_sample = timestamp_sample; report.device_id = _device_id; @@ -165,7 +171,7 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float if (_integrator.put(timestamp_sample, val_calibrated, delta_angle, integral_dt)) { // fill sensor_gyro_integrated and publish - sensor_gyro_integrated_s report{}; + sensor_gyro_integrated_s report; report.timestamp_sample = timestamp_sample; report.error_count = _error_count; @@ -207,7 +213,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) // Apply range scale and the calibration offset const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset}; - sensor_gyro_s report{}; + sensor_gyro_s report; report.timestamp_sample = sample.timestamp_sample; report.device_id = _device_id; @@ -268,7 +274,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample) delta_angle *= 1e-6f * dt; // fill sensor_gyro_integrated and publish - sensor_gyro_integrated_s report{}; + sensor_gyro_integrated_s report; report.timestamp_sample = sample.timestamp_sample; report.error_count = _error_count; @@ -315,13 +321,13 @@ void PX4Gyroscope::PublishStatus() { // publish sensor status if (hrt_elapsed_time(&_status_last_publish) >= 100_ms) { - sensor_gyro_status_s status{}; + sensor_gyro_status_s status; status.device_id = _device_id; status.error_count = _error_count; status.full_scale_range = _range; status.rotation = _rotation; - status.measure_rate = _update_rate; + status.measure_rate_hz = _update_rate; status.temperature = _temperature; status.vibration_metric = _vibration_metric; status.coning_vibration = _coning_vibration; @@ -347,11 +353,10 @@ void PX4Gyroscope::ResetIntegrator() void PX4Gyroscope::UpdateClipLimit() { - // 95% of potential max - _clip_limit = (_range / _scale) * 0.95f; + // 99.9% of potential max + _clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX); } - void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle) { // Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 36d0797614..22975c2d16 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -45,7 +46,7 @@ #include #include -class PX4Gyroscope : public cdev::CDev +class PX4Gyroscope : public cdev::CDev, public ModuleParams { public: PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); @@ -55,6 +56,8 @@ public: uint32_t get_device_id() const { return _device_id; } + float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); } + void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); void set_error_count(uint64_t error_count) { _error_count += error_count; } @@ -72,9 +75,9 @@ public: uint8_t samples; // number of samples float dt; // in microseconds - int16_t x[16]; - int16_t y[16]; - int16_t z[16]; + int16_t x[32]; + int16_t y[32]; + int16_t z[32]; }; static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size"); static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size"); @@ -131,4 +134,7 @@ private: uint8_t _integrator_fifo_samples{0}; uint8_t _integrator_clipping{0}; + DEFINE_PARAMETERS( + (ParamInt) _param_imu_gyro_rate_max + ) };