IMU drivers using FIFOs increase max length to 16 and sync similar implementations

- this provides some extra space when the FIFO transfers don't align perfectly
 - I've also made an effort to keep the different drivers (icm20602, icm20608g, ism330ldc) in sync so we can factor out the common portions later once we've confident in the pattern.

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

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

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<uint8_t>(Register::FIFO_R_W) | DIR_READ;

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);

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<uint8_t>(Register::FIFO_R_W) | DIR_READ;

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<uint8_t>(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<ISM330DLC *>(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<uint8_t>(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);

src/drivers/imu/st/ism330dlc/ISM330DLC.hpp

@@ -48,7 +48,6 @@
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/ecl/geo/geo.h>
 #include <lib/perf/perf_counter.h>
-#include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 
 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};
-
 };

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();

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");

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");