diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg index 56c977afbc..78099a5a2f 100644 --- a/msg/sensor_accel_fifo.msg +++ b/msg/sensor_accel_fifo.msg @@ -8,6 +8,6 @@ float32 scale uint8 samples # number of valid samples -int16[8] x # acceleration in the NED X board axis in m/s/s -int16[8] y # acceleration in the NED Y board axis in m/s/s -int16[8] z # acceleration in the NED Z board axis in m/s/s +int16[16] x # acceleration in the NED X board axis in m/s/s +int16[16] y # acceleration in the NED Y board axis in m/s/s +int16[16] z # acceleration in the NED Z board axis in m/s/s diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg index 15024bff96..61111fda80 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[8] x # angular velocity in the NED X board axis in rad/s -int16[8] y # angular velocity in the NED Y board axis in rad/s -int16[8] z # angular velocity in the NED Z board axis in rad/s +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 diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 97deb8c048..17c898e094 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -121,37 +121,31 @@ bool ICM20602::Init() bool ICM20602::Reset() { - for (int i = 0; i < 5; i++) { - // 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: 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); + // 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); + // 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); - // 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)); + // 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)); - 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); + // 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); - // reset done once data is ready - if (reset_done && clksel_done && data_ready) { - return true; - } - } - - return false; + return reset_done && clksel_done && data_ready; } void ICM20602::ResetFIFO() @@ -322,8 +316,8 @@ void ICM20602::Run() perf_count(_fifo_empty_perf); return; - } else if (samples > 32) { - // not a real overflow, but something went wrong + } else if (samples > 16) { + // not technically an overflow, but more samples than we expected perf_count(_fifo_overflow_perf); ResetFIFO(); return; @@ -337,13 +331,13 @@ void ICM20602::Run() } // Transfer data - struct ICM_Report { + struct TransferBuffer { uint8_t cmd; - FIFO::DATA f[32]; // max 32 samples + FIFO::DATA f[16]; // max 16 samples }; - static_assert(sizeof(ICM_Report) == (sizeof(uint8_t) + 32 * sizeof(FIFO::DATA))); // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding - ICM_Report *report = (ICM_Report *)_dma_data_buffer; + TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); memset(report, 0, transfer_size); report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index b7673bf93f..e4ab3d8f38 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -65,7 +65,6 @@ public: void PrintInfo(); private: - int probe() override; static int DataReadyInterruptCallback(int irq, void *context, void *arg); diff --git a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp index bf825881d5..8cc8cf6f21 100644 --- a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp @@ -121,37 +121,31 @@ bool ICM20608G::Init() bool ICM20608G::Reset() { - for (int i = 0; i < 5; i++) { - // 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: 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); + // 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); + // 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); - // 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)); + // 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)); - 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); + // 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); - // reset done once data is ready - if (reset_done && clksel_done && data_ready) { - return true; - } - } - - return false; + return reset_done && clksel_done && data_ready; } void ICM20608G::ResetFIFO() @@ -298,8 +292,8 @@ void ICM20608G::Run() perf_count(_fifo_empty_perf); return; - } else if (samples > 32) { - // not a real overflow, but something went wrong + } else if (samples > 16) { + // not technically an overflow, but more samples than we expected perf_count(_fifo_overflow_perf); ResetFIFO(); return; @@ -313,13 +307,13 @@ void ICM20608G::Run() } // Transfer data - struct ICM_Report { + struct TransferBuffer { uint8_t cmd; - FIFO::DATA f[32]; // max 32 samples + FIFO::DATA f[16]; // max 16 samples }; - static_assert(sizeof(ICM_Report) == (sizeof(uint8_t) + 32 * sizeof(FIFO::DATA))); // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding - ICM_Report *report = (ICM_Report *)_dma_data_buffer; + TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); memset(report, 0, transfer_size); report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp index fb5e9e91a8..6c58bce735 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp @@ -74,18 +74,19 @@ ISM330DLC::~ISM330DLC() perf_free(_drdy_interval_perf); } -int -ISM330DLC::probe() +int ISM330DLC::probe() { - if (RegisterRead(Register::WHO_AM_I) == ISM330DLC_WHO_AM_I) { - return PX4_OK; + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != ISM330DLC_WHO_AM_I) { + PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; } - return PX4_ERROR; + return PX4_OK; } -bool -ISM330DLC::Init() +bool ISM330DLC::Init() { if (SPI::init() != PX4_OK) { PX4_ERR("SPI::init failed"); @@ -110,43 +111,34 @@ ISM330DLC::Init() return true; } -bool -ISM330DLC::Reset() +bool ISM330DLC::Reset() { - for (int i = 0; i < 5; i++) { - // Reset - // CTRL3_C: SW_RESET - // Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1. - RegisterSetBits(Register::CTRL3_C, CTRL3_C_BIT::BDU | CTRL3_C_BIT::IF_INC | CTRL3_C_BIT::SW_RESET); - usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL3_C register returns to 0). + // CTRL3_C: SW_RESET + // Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1. + RegisterSetBits(Register::CTRL3_C, CTRL3_C_BIT::BDU | CTRL3_C_BIT::IF_INC | CTRL3_C_BIT::SW_RESET); + usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL3_C register returns to 0). - // Accelerometer configuration - // CTRL1_XL: Accelerometer 16 G range and ODR 6.66 kHz - RegisterWrite(Register::CTRL1_XL, CTRL1_XL_BIT::ODR_XL_6_66KHZ | CTRL1_XL_BIT::FS_XL_16); - _px4_accel.set_scale(0.488f * (CONSTANTS_ONE_G / 1000.0f)); // 0.488 mg/LSB - _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + // Accelerometer configuration + // CTRL1_XL: Accelerometer 16 G range and ODR 6.66 kHz + RegisterWrite(Register::CTRL1_XL, CTRL1_XL_BIT::ODR_XL_6_66KHZ | CTRL1_XL_BIT::FS_XL_16); + _px4_accel.set_scale(0.488f * (CONSTANTS_ONE_G / 1000.0f)); // 0.488 mg/LSB + _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); - // Gyroscope configuration - // CTRL2_G: Gyroscope 2000 degrees/second and ODR 6.66 kHz - // CTRL6_C: Gyroscope low-pass filter bandwidth 937 Hz (maximum) - RegisterWrite(Register::CTRL2_G, CTRL2_G_BIT::ODR_G_6_66KHZ | CTRL2_G_BIT::FS_G_2000); - RegisterSetBits(Register::CTRL6_C, CTRL6_C_BIT::FTYPE_GYRO_LPF_BW_937_HZ); - _px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB - _px4_gyro.set_range(math::radians(2000.0f)); + // Gyroscope configuration + // CTRL2_G: Gyroscope 2000 degrees/second and ODR 6.66 kHz + // CTRL6_C: Gyroscope low-pass filter bandwidth 937 Hz (maximum) + RegisterWrite(Register::CTRL2_G, CTRL2_G_BIT::ODR_G_6_66KHZ | CTRL2_G_BIT::FS_G_2000); + RegisterSetBits(Register::CTRL6_C, CTRL6_C_BIT::FTYPE_GYRO_LPF_BW_937_HZ); + _px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB + _px4_gyro.set_range(math::radians(2000.0f)); - const bool reset_done = ((RegisterRead(Register::CTRL3_C) & CTRL3_C_BIT::SW_RESET) == 0); + // reset is considered done once data is ready + const bool reset_done = ((RegisterRead(Register::CTRL3_C) & CTRL3_C_BIT::SW_RESET) == 0); - // reset done once data is ready - if (reset_done) { - return true; - } - } - - return false; + return reset_done; } -void -ISM330DLC::ResetFIFO() +void ISM330DLC::ResetFIFO() { perf_count(_fifo_reset_perf); @@ -163,8 +155,7 @@ ISM330DLC::ResetFIFO() RegisterWrite(Register::FIFO_CTRL5, FIFO_CTRL5_BIT::ODR_FIFO_6_66_KHZ | FIFO_CTRL5_BIT::FIFO_MODE_CONTINUOUS); } -uint8_t -ISM330DLC::RegisterRead(Register reg) +uint8_t ISM330DLC::RegisterRead(Register reg) { uint8_t cmd[2] {}; cmd[0] = static_cast(reg) | DIR_READ; @@ -172,15 +163,13 @@ ISM330DLC::RegisterRead(Register reg) return cmd[1]; } -void -ISM330DLC::RegisterWrite(Register reg, uint8_t value) +void ISM330DLC::RegisterWrite(Register reg, uint8_t value) { uint8_t cmd[2] { (uint8_t)reg, value }; transfer(cmd, cmd, sizeof(cmd)); } -void -ISM330DLC::RegisterSetBits(Register reg, uint8_t setbits) +void ISM330DLC::RegisterSetBits(Register reg, uint8_t setbits) { uint8_t val = RegisterRead(reg); @@ -190,8 +179,7 @@ ISM330DLC::RegisterSetBits(Register reg, uint8_t setbits) } } -void -ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits) +void ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits) { uint8_t val = RegisterRead(reg); @@ -201,18 +189,14 @@ ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits) } } -int -ISM330DLC::DataReadyInterruptCallback(int irq, void *context, void *arg) +int ISM330DLC::DataReadyInterruptCallback(int irq, void *context, void *arg) { ISM330DLC *dev = reinterpret_cast(arg); - dev->DataReady(); - return 0; } -void -ISM330DLC::DataReady() +void ISM330DLC::DataReady() { _time_data_ready = hrt_absolute_time(); @@ -223,8 +207,7 @@ ISM330DLC::DataReady() ScheduleNow(); } -void -ISM330DLC::Start() +void ISM330DLC::Start() { Stop(); @@ -249,8 +232,7 @@ ISM330DLC::Start() #endif } -void -ISM330DLC::Stop() +void ISM330DLC::Stop() { #if defined(GPIO_SPI2_DRDY1_ISM330) && false // TODO: enable // Disable data ready callback @@ -262,8 +244,7 @@ ISM330DLC::Stop() #endif } -void -ISM330DLC::Run() +void ISM330DLC::Run() { perf_count(_interval_perf); @@ -291,7 +272,7 @@ ISM330DLC::Run() const uint8_t fifo_pattern = RegisterRead(Register::FIFO_STATUS3); if (fifo_pattern != 0) { - PX4_ERR("check fifo pattern: %d", fifo_pattern); + PX4_DEBUG("check FIFO pattern: %d", fifo_pattern); } // Transfer data @@ -302,20 +283,22 @@ ISM330DLC::Run() perf_count(_fifo_empty_perf); return; - } else if (samples > 8) { + } else if (samples > 16) { // not technically an overflow, but more samples than we expected perf_count(_fifo_overflow_perf); ResetFIFO(); return; } - const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); - - struct ISM_Report { + // Transfer data + struct TransferBuffer { uint8_t cmd; - FIFO::DATA f[8]; // we never transfer more than 8 samples + FIFO::DATA f[16]; // max 16 samples }; - ISM_Report *report = (ISM_Report *)_dma_data_buffer; + static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding + + TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); memset(report, 0, transfer_size); report->cmd = static_cast(Register::FIFO_DATA_OUT_L) | DIR_READ; @@ -329,7 +312,6 @@ ISM330DLC::Run() perf_end(_transfer_perf); static constexpr uint32_t gyro_dt = 1000000 / ST_ISM330DLC::G_ODR; - // estimate timestamp of first sample in the FIFO from number of samples and fill rate const hrt_abstime timestamp_sample = timestamp_fifo_level - ((samples - 1) * gyro_dt); @@ -367,7 +349,7 @@ ISM330DLC::Run() // 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C. const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2]); - const float temperature = (OUT_TEMP / 256.0f) + 25.0f; + const float temperature = ((float)OUT_TEMP / 256.0f) + 25.0f; _px4_accel.set_temperature(temperature); _px4_gyro.set_temperature(temperature); @@ -377,8 +359,7 @@ ISM330DLC::Run() _px4_accel.updateFIFO(accel); } -void -ISM330DLC::PrintInfo() +void ISM330DLC::PrintInfo() { perf_print_counter(_interval_perf); perf_print_counter(_transfer_perf); diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp index f641ecc6a9..e3c91d7674 100644 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp +++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include using ST_ISM330DLC::Register; @@ -57,38 +56,35 @@ class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem { public: ISM330DLC(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); - virtual ~ISM330DLC(); + ~ISM330DLC() override; - bool Init(); - void Start(); - void Stop(); - bool Reset(); - void PrintInfo(); - -protected: - virtual int probe(); + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); private: + int probe() override; - static int DataReadyInterruptCallback(int irq, void *context, void *arg); - void DataReady(); + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); - void Run() override; + void Run() override; - 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); + 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 ResetFIFO(); + void ResetFIFO(); + uint8_t *_dma_data_buffer{nullptr}; - uint8_t *_dma_data_buffer{nullptr}; + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; - - static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval + static constexpr uint32_t _fifo_interval{1000}; // 1000 us sample interval perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; @@ -100,5 +96,4 @@ private: hrt_abstime _time_data_ready{0}; hrt_abstime _time_last_temperature_update{0}; - }; diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index 5e08a6429c..e785e91923 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -210,7 +210,7 @@ void LSM9DS1::Run() perf_count(_fifo_empty_perf); return; - } else if (samples > 32) { + } else if (samples > 16) { // not technically an overflow, but more samples than we expected perf_count(_fifo_overflow_perf); ResetFIFO(); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index dbe34f112f..63bd208e9c 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -76,9 +76,9 @@ public: uint8_t samples; // number of samples float dt; // in microseconds - int16_t x[8]; - int16_t y[8]; - int16_t z[8]; + int16_t x[16]; + int16_t y[16]; + int16_t z[16]; }; static_assert(sizeof(FIFOSample::x) == sizeof(sensor_accel_fifo_s::x), "FIFOSample.x invalid size"); static_assert(sizeof(FIFOSample::y) == sizeof(sensor_accel_fifo_s::y), "FIFOSample.y invalid size"); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index f50b00a13c..dda7927cb2 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -77,9 +77,9 @@ public: uint8_t samples; // number of samples float dt; // in microseconds - int16_t x[8]; - int16_t y[8]; - int16_t z[8]; + int16_t x[16]; + int16_t y[16]; + int16_t z[16]; }; 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");