diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake
index 0c2c8a34660..ad6603c2535 100644
--- a/boards/px4/fmu-v5x/default.cmake
+++ b/boards/px4/fmu-v5x/default.cmake
@@ -32,8 +32,8 @@ px4_add_board(
 		imu/adis16477
 		imu/adis16497
 		imu/bmi088
-# TBD		imu/ism330dlc - needs bus selection
 		imu/mpu6000
+		imu/st/ism330dlc
 		irlock
 		lights/blinkm
 		lights/rgbled
diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp
index 08d39b0262f..3cc7d258df8 100644
--- a/boards/px4/fmu-v5x/src/spi.cpp
+++ b/boards/px4/fmu-v5x/src/spi.cpp
@@ -335,9 +335,7 @@ __EXPORT void board_spi_reset(int mask_ms)
 		stm32_configgpio(GPIO_SPI2_SCK_OFF);
 		stm32_configgpio(GPIO_SPI2_MISO_OFF);
 		stm32_configgpio(GPIO_SPI2_MOSI_OFF);
-#if BOARD_USE_DRDY
 		stm32_configgpio(GPIO_DRDY_OFF_SPI2_DRDY1_ISM330);
-#endif
 		/* set the sensor rail off */
 		stm32_gpiowrite(GPIO_VDD_3V3_SENSORS2_EN, 0);
 	}
@@ -412,9 +410,7 @@ __EXPORT void board_spi_reset(int mask_ms)
 		stm32_configgpio(GPIO_SPI2_SCK);
 		stm32_configgpio(GPIO_SPI2_MISO);
 		stm32_configgpio(GPIO_SPI2_MOSI);
-#if BOARD_USE_DRDY
 		stm32_configgpio(GPIO_SPI2_DRDY1_ISM330);
-#endif
 	}
 
 	if (mask & 4) {
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 43cff31fc54..ede08861f14 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -45,8 +45,8 @@ set(msg_files
 	camera_capture.msg
 	camera_trigger.msg
 	cellular_status.msg
-	collision_report.msg
 	collision_constraints.msg
+	collision_report.msg
 	commander_state.msg
 	cpuload.msg
 	debug_array.msg
@@ -104,12 +104,16 @@ set(msg_files
 	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
+	sensor_accel_fifo.msg
+	sensor_accel_status.msg
 	sensor_baro.msg
 	sensor_bias.msg
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gyro.msg
 	sensor_gyro_control.msg
+	sensor_gyro_fifo.msg
+	sensor_gyro_status.msg
 	sensor_mag.msg
 	sensor_preflight.msg
 	sensor_selection.msg
diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg
index 18ca1b96d17..119fa7fd124 100644
--- a/msg/sensor_accel.msg
+++ b/msg/sensor_accel.msg
@@ -7,14 +7,16 @@ float32 x		# acceleration in the NED X board axis in m/s^2
 float32 y		# acceleration in the NED Y board axis in m/s^2
 float32 z		# acceleration in the NED Z board axis in m/s^2
 
-uint32 integral_dt	# integration time  (microseconds)
+uint32 integral_dt	# integration time (microseconds)
+uint8 integral_samples	# number of samples integrated
 float32 x_integral	# delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt)
 float32 y_integral	# delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt)
 float32 z_integral	# delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt)
+uint8 integral_clip_count	# total clip count per integration period on any axis
 
 float32 temperature	# temperature in degrees celsius
 
-float32 scaling		# scaling from raw to m/s^s
+float32 scaling		# scaling from raw to m/s^2
 int16 x_raw
 int16 y_raw
 int16 z_raw
diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg
new file mode 100644
index 00000000000..56c977afbcd
--- /dev/null
+++ b/msg/sensor_accel_fifo.msg
@@ -0,0 +1,13 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp_sample	# time since system start (microseconds)
+
+float32 dt		# delta time between samples (microseconds)
+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
diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg
new file mode 100644
index 00000000000..d670906c7f1
--- /dev/null
+++ b/msg/sensor_accel_status.msg
@@ -0,0 +1,18 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 error_count
+
+float32 temperature
+
+uint8 rotation
+
+# clipping per axis?
+uint64[3] clipping
+
+uint16 measure_rate
+uint16 sample_rate
+
+float32 full_scale_range
+
+float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)
diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg
index 5a7ec71d788..b4ff35c666c 100644
--- a/msg/sensor_gyro.msg
+++ b/msg/sensor_gyro.msg
@@ -1,6 +1,5 @@
-uint32 device_id	# unique device ID for the sensor that does not change between power cycles
-
 uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
 
 uint64 error_count
 
@@ -9,9 +8,11 @@ float32 y		# angular velocity in the NED Y board axis in rad/s
 float32 z		# angular velocity in the NED Z board axis in rad/s
 
 uint32 integral_dt	# integration time (microseconds)
+uint8 integral_samples	# number of samples integrated
 float32 x_integral	# delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt)
 float32 y_integral	# delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt)
 float32 z_integral	# delta angle in the NED Z board axis in rad/s over the integration time frame (integral_dt)
+uint8 integral_clip_count	# total clip count per integration period on any axis
 
 float32 temperature	# temperature in degrees celsius
 
diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg
new file mode 100644
index 00000000000..15024bff968
--- /dev/null
+++ b/msg/sensor_gyro_fifo.msg
@@ -0,0 +1,13 @@
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample	# time since system start (microseconds)
+
+float32 dt		# delta time between samples (microseconds)
+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
diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg
new file mode 100644
index 00000000000..dfa8521e2d4
--- /dev/null
+++ b/msg/sensor_gyro_status.msg
@@ -0,0 +1,20 @@
+uint64 timestamp	# time since system start (microseconds)
+uint32 device_id	# unique device ID for the sensor that does not change between power cycles
+
+uint64 error_count
+
+float32 temperature
+
+uint8 rotation
+
+# clipping per axis?
+uint64[3] clipping
+
+uint16 measure_rate
+uint16 sample_rate
+
+float32 full_scale_range
+
+float32 coning_vibration	# Level of coning vibration in the IMU delta angles (rad^2)
+
+float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)
diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml
index 91164ca810b..5f8a78203aa 100644
--- a/msg/tools/uorb_rtps_message_ids.yaml
+++ b/msg/tools/uorb_rtps_message_ids.yaml
@@ -265,6 +265,14 @@ rtps:
     id: 116
   - msg: cellular_status
     id: 117
+  - msg: sensor_accel_fifo
+    id: 118
+  - msg: sensor_accel_status
+    id: 119
+  - msg: sensor_gyro_fifo
+    id: 120
+  - msg: sensor_gyro_status
+    id: 121
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 150
diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h
index 4a3ab76a2ba..4de616f40e7 100644
--- a/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h
+++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h
@@ -46,6 +46,8 @@
 
 #include <board_config.h>
 
+__BEGIN_DECLS
+
 /************************************************************************************
  * Name: board_dma_alloc_init
  *
@@ -114,7 +116,7 @@ __EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
 __EXPORT void *board_dma_alloc(size_t size);
 #else
-#define board_dma_alloc(size) (NULL)
+#define board_dma_alloc(size) malloc(size)
 #endif
 
 /************************************************************************************
@@ -131,5 +133,7 @@ __EXPORT void *board_dma_alloc(size_t size);
 #if defined(BOARD_DMA_ALLOC_POOL_SIZE)
 __EXPORT void board_dma_free(FAR void *memory, size_t size);
 #else
-#define board_dma_free(memory, size) ()
+#define board_dma_free(memory, size) free(memory)
 #endif
+
+__END_DECLS
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 20c90f68447..3ac52f4160a 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -117,6 +117,7 @@
 #define DRV_DEVTYPE_BMI088		0x66
 #define DRV_DEVTYPE_BMP388		0x67
 #define DRV_DEVTYPE_DPS310		0x68
+#define DRV_DEVTYPE_ST_ISM330DLC	0x69
 
 /*
  * ioctl() definitions
diff --git a/src/drivers/imu/st/ism330dlc/CMakeLists.txt b/src/drivers/imu/st/ism330dlc/CMakeLists.txt
new file mode 100644
index 00000000000..8094a4c3f24
--- /dev/null
+++ b/src/drivers/imu/st/ism330dlc/CMakeLists.txt
@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__imu__st__ism330dlc
+	MAIN ism330dlc
+	SRCS
+		ism330dlc_main.cpp
+		ISM330DLC.cpp
+		ISM330DLC.hpp
+		ST_ISM330DLC_Registers.hpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)
diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
new file mode 100644
index 00000000000..fb5e9e91a8b
--- /dev/null
+++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp
@@ -0,0 +1,391 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ISM330DLC.hpp"
+
+#include <px4_platform/board_dma_alloc.h>
+
+using namespace time_literals;
+using namespace ST_ISM330DLC;
+
+static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
+
+ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
+	SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED),
+	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
+	_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
+	_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
+{
+	set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
+	_px4_accel.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
+	_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
+
+	_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
+	_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
+
+	_px4_accel.set_update_rate(1000000 / _fifo_interval);
+	_px4_gyro.set_update_rate(1000000 / _fifo_interval);
+}
+
+ISM330DLC::~ISM330DLC()
+{
+	Stop();
+
+	if (_dma_data_buffer != nullptr) {
+		board_dma_free(_dma_data_buffer, FIFO::SIZE);
+	}
+
+	perf_free(_interval_perf);
+	perf_free(_transfer_perf);
+	perf_free(_fifo_empty_perf);
+	perf_free(_fifo_overflow_perf);
+	perf_free(_fifo_reset_perf);
+	perf_free(_drdy_count_perf);
+	perf_free(_drdy_interval_perf);
+}
+
+int
+ISM330DLC::probe()
+{
+	if (RegisterRead(Register::WHO_AM_I) == ISM330DLC_WHO_AM_I) {
+		return PX4_OK;
+	}
+
+	return PX4_ERROR;
+}
+
+bool
+ISM330DLC::Init()
+{
+	if (SPI::init() != PX4_OK) {
+		PX4_ERR("SPI::init failed");
+		return false;
+	}
+
+	if (!Reset()) {
+		PX4_ERR("reset failed");
+		return false;
+	}
+
+	// allocate DMA capable buffer
+	_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
+
+	if (_dma_data_buffer == nullptr) {
+		PX4_ERR("DMA alloc failed");
+		return false;
+	}
+
+	Start();
+
+	return true;
+}
+
+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).
+
+		// 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));
+
+		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;
+}
+
+void
+ISM330DLC::ResetFIFO()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_CTRL5 - disable FIFO
+	RegisterWrite(Register::FIFO_CTRL5, 0);
+
+	// CTRL5_C: rounding mode gyro + accel
+	RegisterWrite(Register::CTRL5_C, CTRL5_C_BIT::ROUNDING_GYRO_ACCEL);
+
+	// FIFO_CTRL3: full gyro and accel data to FIFO
+	RegisterWrite(Register::FIFO_CTRL3, FIFO_CTRL3_BIT::DEC_FIFO_GYRO | FIFO_CTRL3_BIT::DEC_FIFO_XL);
+
+	// FIFO_CTRL5: FIFO ODR is set to 6.66 kHz, and FIFO continuous mode enabled
+	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 cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+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)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (!(val & setbits)) {
+		val |= setbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+void
+ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits)
+{
+	uint8_t val = RegisterRead(reg);
+
+	if (val & clearbits) {
+		val &= !clearbits;
+		RegisterWrite(reg, val);
+	}
+}
+
+int
+ISM330DLC::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	ISM330DLC *dev = reinterpret_cast<ISM330DLC *>(arg);
+
+	dev->DataReady();
+
+	return 0;
+}
+
+void
+ISM330DLC::DataReady()
+{
+	_time_data_ready = hrt_absolute_time();
+
+	perf_count(_drdy_count_perf);
+	perf_count(_drdy_interval_perf);
+
+	// make another measurement
+	ScheduleNow();
+}
+
+void
+ISM330DLC::Start()
+{
+	Stop();
+
+	ResetFIFO();
+
+#if defined(GPIO_SPI2_DRDY1_ISM330) && false	// TODO: enable
+	// Setup data ready on rising edge
+	px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this);
+
+	// FIFO threshold level setting
+	// FIFO_CTRL1: FTH_[7:0]
+	// FIFO_CTRL2: FTH_[10:8]
+	const uint8_t fifo_threshold = 12;
+	RegisterWrite(Register::FIFO_CTRL1, fifo_threshold);
+
+	// INT1: FIFO full, overrun, or threshold
+	RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR |
+		      INT1_CTRL_BIT::INT1_FTH);
+#else
+
+	ScheduleOnInterval(_fifo_interval, _fifo_interval);
+#endif
+}
+
+void
+ISM330DLC::Stop()
+{
+#if defined(GPIO_SPI2_DRDY1_ISM330) && false	// TODO: enable
+	// Disable data ready callback
+	px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ISM330, false, false, false, nullptr, nullptr);
+
+	RegisterWrite(Register::INT1_CTRL, 0);
+#else
+	ScheduleClear();
+#endif
+}
+
+void
+ISM330DLC::Run()
+{
+	perf_count(_interval_perf);
+
+	// Number of unread words (16-bit axes) stored in FIFO.
+	const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
+	const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1);
+
+	// check for FIFO status
+	const uint8_t FIFO_STATUS2 = RegisterRead(Register::FIFO_STATUS2);
+
+	if (FIFO_STATUS2 & FIFO_STATUS2_BIT::OVER_RUN) {
+		// overflow
+		perf_count(_fifo_overflow_perf);
+		ResetFIFO();
+		return;
+
+	} else if (FIFO_STATUS2 & FIFO_STATUS2_BIT::FIFO_EMPTY) {
+		// fifo empty could indicate a problem, reset
+		perf_count(_fifo_empty_perf);
+		ResetFIFO();
+		return;
+	}
+
+	// FIFO pattern: indicates Next reading from FIFO output registers (Gx, Gy, Gz, XLx, XLy, XLz)
+	const uint8_t fifo_pattern = RegisterRead(Register::FIFO_STATUS3);
+
+	if (fifo_pattern != 0) {
+		PX4_ERR("check fifo pattern: %d", fifo_pattern);
+	}
+
+	// Transfer data
+	// only transfer out complete sets of gyro + accel
+	const int samples = (fifo_words / 2) / sizeof(FIFO::DATA);
+
+	if (samples < 1) {
+		perf_count(_fifo_empty_perf);
+		return;
+
+	} else if (samples > 8) {
+		// 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 {
+		uint8_t cmd;
+		FIFO::DATA f[8]; // we never transfer more than 8 samples
+	};
+	ISM_Report *report = (ISM_Report *)_dma_data_buffer;
+	memset(report, 0, transfer_size);
+	report->cmd = static_cast<uint8_t>(Register::FIFO_DATA_OUT_L) | DIR_READ;
+
+	perf_begin(_transfer_perf);
+
+	if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
+		perf_end(_transfer_perf);
+		return;
+	}
+
+	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);
+
+	PX4Accelerometer::FIFOSample accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = samples;
+	accel.dt = gyro_dt;
+
+	PX4Gyroscope::FIFOSample gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = samples;
+	gyro.dt = gyro_dt;
+
+	for (int i = 0; i < samples; i++) {
+		const FIFO::DATA &fifo_sample = report->f[i];
+
+		// sensor Z is up (RHC), flip y & z for publication
+		gyro.x[i] = combine(fifo_sample.OUTX_L_G, fifo_sample.OUTX_H_G);
+		gyro.y[i] = -combine(fifo_sample.OUTY_L_G, fifo_sample.OUTY_H_G);
+		gyro.z[i] = -combine(fifo_sample.OUTZ_L_G, fifo_sample.OUTZ_H_G);
+
+		accel.x[i] = combine(fifo_sample.OUTX_L_XL, fifo_sample.OUTX_H_XL);
+		accel.y[i] = -combine(fifo_sample.OUTY_L_XL, fifo_sample.OUTY_H_XL);
+		accel.z[i] = -combine(fifo_sample.OUTZ_L_XL, fifo_sample.OUTZ_H_XL);
+	}
+
+	// get current temperature at 1 Hz
+	if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
+		uint8_t temperature_buf[3] {};
+		temperature_buf[0] = static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ;
+
+		if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
+			return;
+		}
+
+		// 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;
+
+		_px4_accel.set_temperature(temperature);
+		_px4_gyro.set_temperature(temperature);
+	}
+
+	_px4_gyro.updateFIFO(gyro);
+	_px4_accel.updateFIFO(accel);
+}
+
+void
+ISM330DLC::PrintInfo()
+{
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_transfer_perf);
+	perf_print_counter(_fifo_empty_perf);
+	perf_print_counter(_fifo_overflow_perf);
+	perf_print_counter(_fifo_reset_perf);
+
+	_px4_accel.print_status();
+	_px4_gyro.print_status();
+}
diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
new file mode 100644
index 00000000000..f641ecc6a9e
--- /dev/null
+++ b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ISM330DLC.hpp
+ *
+ * Driver for the ST ISM330DLC connected via SPI.
+ *
+ */
+
+#pragma once
+
+#include "ST_ISM330DLC_Registers.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/device/spi.h>
+#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;
+
+class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem
+{
+public:
+	ISM330DLC(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
+	virtual ~ISM330DLC();
+
+	bool		Init();
+	void		Start();
+	void		Stop();
+	bool		Reset();
+	void		PrintInfo();
+
+protected:
+	virtual int	probe();
+
+private:
+
+	static int	DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void		DataReady();
+
+	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);
+
+	void		ResetFIFO();
+
+
+	uint8_t	*_dma_data_buffer{nullptr};
+
+	PX4Accelerometer	_px4_accel;
+	PX4Gyroscope		_px4_gyro;
+
+	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")};
+	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_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")};
+	perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")};
+
+	hrt_abstime _time_data_ready{0};
+	hrt_abstime _time_last_temperature_update{0};
+
+};
diff --git a/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp b/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp
new file mode 100644
index 00000000000..b6c32c0a453
--- /dev/null
+++ b/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp
@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ST_ISM330DLC_registers.hpp
+ *
+ * ST ISM330DLC registers.
+ *
+ */
+
+#pragma once
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+namespace ST_ISM330DLC
+{
+
+static constexpr uint8_t DIR_READ	= 0x80;
+
+static constexpr uint8_t ISM330DLC_WHO_AM_I = 0b01101010; // Who I am ID
+
+static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
+
+static constexpr uint32_t LA_ODR = 6664;	// Linear acceleration output data rate
+static constexpr uint32_t G_ODR = 6664;		// Angular rate output data rate
+
+enum class
+Register : uint8_t {
+	FIFO_CTRL1	= 0x06,	// FIFO threshold level setting.
+
+	FIFO_CTRL3	= 0x08,	// FIFO control register (r/w).
+
+	FIFO_CTRL5	= 0x0A,
+
+	INT1_CTRL	= 0x0D,
+	INT2_CTRL	= 0x0E,
+	WHO_AM_I	= 0x0F,
+
+	CTRL1_XL	= 0x10,	// Linear acceleration sensor control register 1 (r/w).
+	CTRL2_G		= 0x11,	// Angular rate sensor control register 2 (r/w).
+	CTRL3_C		= 0x12,	// Control register 3 (r/w).
+	CTRL4_C		= 0x13,
+	CTRL5_C		= 0x14, // Control register 5 (r/w).
+	CTRL6_C		= 0x15,	// Angular rate sensor control register 6 (r/w).
+
+	OUT_TEMP_L	= 0x20,
+	OUT_TEMP_H	= 0x21,
+
+	FIFO_STATUS1	= 0x3A,	// FIFO status control register (r)
+	FIFO_STATUS2	= 0x3B,	// FIFO status control register (r)
+	FIFO_STATUS3	= 0x3C,	// FIFO status control register (r)
+
+	FIFO_DATA_OUT_L	= 0x3E,	// FIFO data output (first byte)
+	FIFO_DATA_OUT_H	= 0x3F,	// FIFO data output (second byte)
+};
+
+
+// FIFO_CTRL3
+enum
+FIFO_CTRL3_BIT : uint8_t {
+	DEC_FIFO_GYRO	= Bit3,	// Gyro no decimation
+	DEC_FIFO_XL	= Bit0,	// Accel no decimation
+};
+
+// FIFO_CTRL5
+enum
+FIFO_CTRL5_BIT : uint8_t {
+	ODR_FIFO_6_66_KHZ	= Bit6 | Bit4,	// FIFO ODR is set to 6.66 kHz
+
+	FIFO_MODE_CONTINUOUS	= Bit2 | Bit1,	// Continuous mode. If the FIFO is full, the new sample overwrites the older one.
+
+};
+
+// INT1_CTRL
+enum
+INT1_CTRL_BIT : uint8_t {
+	INT1_FULL_FLAG		= Bit5,
+	INT1_FIFO_OVR		= Bit4,
+	INT1_FTH		= Bit3,
+
+	INT1_DRDY_G		= Bit1,
+	INT1_DRDY_XL		= Bit0,
+};
+
+// INT2_CTRL
+enum
+INT2_CTRL_BIT : uint8_t {
+	INT2_FULL_FLAG		= Bit5,
+	INT2_FIFO_OVR		= Bit4,
+	INT2_FTH		= Bit3,
+
+	INT2_DRDY_G		= Bit1,
+	INT2_DRDY_XL		= Bit0,
+};
+
+// CTRL1_XL
+enum
+CTRL1_XL_BIT : uint8_t {
+	ODR_XL_6_66KHZ	= Bit7 | Bit5,	// 6.66 kHz Output data rate and power mode selection
+
+	FS_XL_16	= Bit2,		// FS_XL 01: ±16 g
+};
+
+// CTRL2_G
+enum
+CTRL2_G_BIT : uint8_t {
+	ODR_G_6_66KHZ	= Bit7 | Bit5,
+
+	FS_G_2000	= Bit3 | Bit2,
+};
+
+// CTRL3_C
+enum
+CTRL3_C_BIT : uint8_t {
+	BDU		= Bit7,
+
+	IF_INC		= Bit2,
+
+	SW_RESET	= Bit0
+};
+
+// CTRL4_C
+enum
+CTRL4_C_BIT : uint8_t {
+	INT2_on_INT1	= Bit5,
+};
+
+// CTRL5_C
+enum
+CTRL5_C_BIT : uint8_t {
+	ROUNDING_GYRO_ACCEL = Bit1 | Bit0,	// ROUNDING[2:0] - 011 Gyroscope + accelerometer
+};
+
+// CTRL6_C
+enum
+CTRL6_C_BIT : uint8_t {
+	FTYPE_GYRO_LPF_BW_937_HZ = Bit1 | Bit0
+};
+
+// FIFO_STATUS2
+enum
+FIFO_STATUS2_BIT : uint8_t {
+	OVER_RUN	= Bit6,
+
+	FIFO_EMPTY	= Bit4,
+};
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 4096;
+
+// Saving data in the FIFO buffer is organized in four FIFO data sets consisting of 6 bytes each
+// each FIFO sample is composed of 16 bits
+struct DATA {
+	uint8_t OUTX_L_G;
+	uint8_t OUTX_H_G;
+	uint8_t OUTY_L_G;
+	uint8_t OUTY_H_G;
+	uint8_t OUTZ_L_G;
+	uint8_t OUTZ_H_G;
+
+	uint8_t OUTX_L_XL;
+	uint8_t OUTX_H_XL;
+	uint8_t OUTY_L_XL;
+	uint8_t OUTY_H_XL;
+	uint8_t OUTZ_L_XL;
+	uint8_t OUTZ_H_XL;
+};
+static_assert(sizeof(DATA) == 12);
+}
+
+} // namespace ST_ISM330DLC
diff --git a/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp b/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp
new file mode 100644
index 00000000000..9d24e7ec1af
--- /dev/null
+++ b/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "ISM330DLC.hpp"
+
+#include <px4_platform_common/getopt.h>
+
+namespace ism330dlc
+{
+
+ISM330DLC *g_dev{nullptr};
+
+int	start(enum Rotation rotation);
+int	stop();
+int	status();
+void	usage();
+
+int start(enum Rotation rotation)
+{
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+		return 0;
+	}
+
+	// create the driver
+	g_dev = new ISM330DLC(PX4_SPI_BUS_SENSORS2, PX4_SPIDEV_ISM330, rotation);	// v5x TODO: board manifest
+
+	if (g_dev == nullptr) {
+		PX4_ERR("driver start failed");
+		return -1;
+	}
+
+	if (!g_dev->Init()) {
+		PX4_ERR("driver init failed");
+		delete g_dev;
+		g_dev = nullptr;
+		return -1;
+	}
+
+	return 0;
+}
+
+int stop()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+	}
+
+	g_dev->Stop();
+	delete g_dev;
+
+	return 0;
+}
+
+int reset()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+	}
+
+	return g_dev->Reset();
+}
+
+int status()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+		return -1;
+	}
+
+	g_dev->PrintInfo();
+
+	return 0;
+}
+
+void usage()
+{
+	PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'");
+	PX4_INFO("options:");
+	PX4_INFO("    -R rotation");
+}
+
+} // namespace ism330dlc
+
+extern "C" __EXPORT int ism330dlc_main(int argc, char *argv[])
+{
+	enum Rotation rotation = ROTATION_NONE;
+	int myoptind = 1;
+	int ch = 0;
+	const char *myoptarg = nullptr;
+
+	/* start options */
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (enum Rotation)atoi(myoptarg);
+			break;
+
+		default:
+			ism330dlc::usage();
+			return 0;
+		}
+	}
+
+	const char *verb = argv[myoptind];
+
+	if (!strcmp(verb, "start")) {
+		return ism330dlc::start(rotation);
+
+	} else if (!strcmp(verb, "stop")) {
+		return ism330dlc::stop();
+
+	} else if (!strcmp(verb, "status")) {
+		return ism330dlc::status();
+
+	} else if (!strcmp(verb, "reset")) {
+		return ism330dlc::reset();
+	}
+
+	ism330dlc::usage();
+
+	return 0;
+}
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
index 8b74a8122e1..cb7301fd64c 100644
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
@@ -36,20 +36,23 @@
 
 #include <lib/drivers/device/Device.hpp>
 
+using namespace time_literals;
+using matrix::Vector3f;
+
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
-	_sensor_accel_pub{ORB_ID(sensor_accel), priority},
+	_sensor_pub{ORB_ID(sensor_accel), priority},
+	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
+	_sensor_status_pub{ORB_ID(sensor_accel_status), priority},
+	_device_id{device_id},
 	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
 
-	_sensor_accel_pub.get().device_id = device_id;
-	_sensor_accel_pub.get().scaling = 1.0f;
-
 	// set software low pass filter for controllers
 	updateParams();
-	configure_filter(_param_imu_accel_cutoff.get());
+	ConfigureFilter(_param_imu_accel_cutoff.get());
 }
 
 PX4Accelerometer::~PX4Accelerometer()
@@ -68,14 +71,14 @@ PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 			accel_calibration_s cal{};
 			memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal));
 
-			_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-			_calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
+			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
+			_calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
 		}
 
 		return PX4_OK;
 
 	case DEVIOCGDEVICEID:
-		return _sensor_accel_pub.get().device_id;
+		return _device_id;
 
 	default:
 		return -ENOTTY;
@@ -87,45 +90,58 @@ PX4Accelerometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
-	device_id.devid = _sensor_accel_pub.get().device_id;
+	device_id.devid = _device_id;
 
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
 	// copy back to report
-	_sensor_accel_pub.get().device_id = device_id.devid;
+	_device_id = device_id.devid;
 }
 
 void
-PX4Accelerometer::set_sample_rate(unsigned rate)
+PX4Accelerometer::set_sample_rate(uint16_t rate)
 {
 	_sample_rate = rate;
-	_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
+
+	ConfigureFilter(_filter.get_cutoff_freq());
+}
+
+void
+PX4Accelerometer::set_update_rate(uint16_t rate)
+{
+	const uint32_t update_interval = 1000000 / rate;
+
+	_integrator_reset_samples = 4000 / update_interval;
 }
 
 void
 PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
 {
-	sensor_accel_s &report = _sensor_accel_pub.get();
-	report.timestamp = timestamp;
-
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
-	const matrix::Vector3f raw{x, y, z};
+	const Vector3f raw{x, y, z};
 
 	// Apply range scale and the calibrating offset/scale
-	const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
+	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
 
 	// Filtered values
-	const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
+	const Vector3f val_filtered{_filter.apply(val_calibrated)};
 
 	// Integrated values
-	matrix::Vector3f integrated_value;
+	Vector3f integrated_value;
 	uint32_t integral_dt = 0;
 
 	if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
 
+		sensor_accel_s report{};
+		report.timestamp = timestamp;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		report.scaling = _scale;
+		report.error_count = _error_count;
+
 		// Raw values (ADC units 0 - 65535)
 		report.x_raw = x;
 		report.y_raw = y;
@@ -140,11 +156,190 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
 		report.y_integral = integrated_value(1);
 		report.z_integral = integrated_value(2);
 
-		poll_notify(POLLIN);
-		_sensor_accel_pub.update();
+		_sensor_pub.publish(report);
 	}
 }
 
+void
+PX4Accelerometer::updateFIFO(const FIFOSample &sample)
+{
+	// filtered data (control)
+	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
+	float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
+	float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
+
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
+
+	const Vector3f raw{x_filtered, y_filtered, z_filtered};
+
+	// Apply range scale and the calibrating offset/scale
+	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
+
+
+	// status
+	{
+		sensor_accel_status_s &status = _sensor_status_pub.get();
+
+		const int16_t clip_limit = (_range / _scale) * 0.95f;
+
+		// x clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.x[n]) > clip_limit) {
+				status.clipping[0]++;
+				_integrator_clipping++;
+			}
+		}
+
+		// y clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.y[n]) > clip_limit) {
+				status.clipping[1]++;
+				_integrator_clipping++;
+			}
+		}
+
+		// z clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.z[n]) > clip_limit) {
+				status.clipping[2]++;
+				_integrator_clipping++;
+			}
+		}
+
+		status.device_id = _device_id;
+		status.error_count = _error_count;
+		status.full_scale_range = _range;
+		status.rotation = _rotation;
+		status.measure_rate = _update_rate;
+		status.sample_rate = _sample_rate;
+		status.temperature = _temperature;
+		status.timestamp = hrt_absolute_time();
+		_sensor_status_pub.publish(status);
+	}
+
+
+	// integrated data (INS)
+	{
+		// reset integrator if previous sample was too long ago
+		if ((sample.timestamp_sample > _timestamp_sample_prev)
+		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
+
+			ResetIntegrator();
+		}
+
+		if (_integrator_samples == 0) {
+			_integrator_timestamp_sample = sample.timestamp_sample;
+		}
+
+		// integrate
+		_integrator_samples += 1;
+		_integrator_fifo_samples += sample.samples;
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[0] += sample.x[n];
+		}
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[1] += sample.y[n];
+		}
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[2] += sample.z[n];
+		}
+
+		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
+
+			const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
+
+			// average integrated values to apply calibration
+			float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
+			float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
+			float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
+
+			// Apply rotation (before scaling)
+			rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
+
+			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
+
+			// Apply range scale and the calibrating offset/scale
+			Vector3f val_int_calibrated{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))};
+			val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+
+			// publish
+			sensor_accel_s report{};
+			report.device_id = _device_id;
+			report.temperature = _temperature;
+			report.scaling = _scale;
+			report.error_count = _error_count;
+
+			// Raw values (ADC units 0 - 65535)
+			report.x_raw = sample.x[0];
+			report.y_raw = sample.y[0];
+			report.z_raw = sample.z[0];
+
+			report.x = val_calibrated(0);
+			report.y = val_calibrated(1);
+			report.z = val_calibrated(2);
+
+			report.integral_dt = integrator_dt_us;
+			report.integral_samples = _integrator_fifo_samples;
+			report.x_integral = val_int_calibrated(0);
+			report.y_integral = val_int_calibrated(1);
+			report.z_integral = val_int_calibrated(2);
+			report.integral_clip_count = _integrator_clipping;
+
+			report.timestamp = _integrator_timestamp_sample;
+			_sensor_pub.publish(report);
+
+
+			// reset integrator
+			ResetIntegrator();
+		}
+
+		_timestamp_sample_prev = sample.timestamp_sample;
+	}
+
+	sensor_accel_fifo_s fifo{};
+
+	fifo.device_id = _device_id;
+	fifo.timestamp_sample = sample.timestamp_sample;
+	fifo.dt = sample.dt;
+	fifo.scale = _scale;
+	fifo.samples = sample.samples;
+
+	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
+	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
+	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
+
+	fifo.timestamp = hrt_absolute_time();
+	_sensor_fifo_pub.publish(fifo);
+}
+
+void
+PX4Accelerometer::ResetIntegrator()
+{
+	_integrator_samples = 0;
+	_integrator_fifo_samples = 0;
+	_integrator_accum[0] = 0;
+	_integrator_accum[1] = 0;
+	_integrator_accum[2] = 0;
+	_integrator_clipping = 0;
+
+	_integrator_timestamp_sample = 0;
+	_timestamp_sample_prev = 0;
+}
+
+void
+PX4Accelerometer::ConfigureFilter(float cutoff_freq)
+{
+	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
+
+	_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
+	_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
+	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
+}
+
 void
 PX4Accelerometer::print_status()
 {
@@ -157,5 +352,4 @@ PX4Accelerometer::print_status()
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
 
-	print_message(_sensor_accel_pub.get());
 }
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
index 5dbf15c2873..94301752764 100644
--- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp
@@ -33,16 +33,19 @@
 
 #pragma once
 
-#include <drivers/device/integrator.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_hrt.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
-#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/drivers/device/integrator.h>
+#include <lib/ecl/geo/geo.h>
+#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <px4_platform_common/module_params.h>
-#include <uORB/uORB.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_accel_fifo.h>
+#include <uORB/topics/sensor_accel_status.h>
 
 class PX4Accelerometer : public cdev::CDev, public ModuleParams
 {
@@ -53,34 +56,80 @@ public:
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
 
-	void set_device_type(uint8_t devtype);
-	void set_error_count(uint64_t error_count) { _sensor_accel_pub.get().error_count = error_count; }
-	void set_scale(float scale) { _sensor_accel_pub.get().scaling = scale; }
-	void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; }
+	uint32_t get_device_id() const { return _device_id; }
 
-	void set_sample_rate(unsigned rate);
+	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; }
+	void set_range(float range) { _range = range; }
+	void set_sample_rate(uint16_t rate);
+	void set_scale(float scale) { _scale = scale; }
+	void set_temperature(float temperature) { _temperature = temperature; }
+	void set_update_rate(uint16_t rate);
 
 	void update(hrt_abstime timestamp, float x, float y, float z);
 
 	void print_status();
 
+	struct FIFOSample {
+		hrt_abstime timestamp_sample;
+		uint8_t samples; // number of samples
+		float dt; // in microseconds
+
+		int16_t x[8];
+		int16_t y[8];
+		int16_t z[8];
+	};
+	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");
+	static_assert(sizeof(FIFOSample::z) == sizeof(sensor_accel_fifo_s::z), "FIFOSample.z invalid size");
+
+	void updateFIFO(const FIFOSample &sample);
+
 private:
 
-	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+	void		ConfigureFilter(float cutoff_freq);
+	void		ResetIntegrator();
 
-	uORB::PublicationMultiData<sensor_accel_s>	_sensor_accel_pub;
+	uORB::PublicationMulti<sensor_accel_s>			_sensor_pub;		// legacy message
+	uORB::PublicationMulti<sensor_accel_fifo_s>		_sensor_fifo_pub;
+	uORB::PublicationMultiData<sensor_accel_status_s>	_sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
-	Integrator _integrator{4000, false};
 
-	const enum Rotation	_rotation;
+	math::LowPassFilter2pArray _filterArrayX{4000, 100};
+	math::LowPassFilter2pArray _filterArrayY{4000, 100};
+	math::LowPassFilter2pArray _filterArrayZ{4000, 100};
+
+	Integrator		_integrator{4000, false};
 
 	matrix::Vector3f	_calibration_scale{1.0f, 1.0f, 1.0f};
 	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};
 
 	int			_class_device_instance{-1};
 
-	unsigned		_sample_rate{1000};
+
+	uint32_t		_device_id{0};
+
+	const enum Rotation	_rotation;
+
+	float			_range{16.0f * CONSTANTS_ONE_G};
+	float			_scale{1.0f};
+	float			_temperature{0.0f};
+
+	uint64_t		_error_count{0};
+
+	uint16_t		_sample_rate{1000};
+	uint16_t		_update_rate{1000};
+
+	// integrator
+	hrt_abstime		_integrator_timestamp_sample{0};
+	hrt_abstime		_timestamp_sample_prev{0};
+	int32_t			_integrator_accum[3] {};
+	uint8_t			_integrator_reset_samples{4};
+	uint8_t			_integrator_samples{0};
+	uint8_t			_integrator_fifo_samples{0};
+	uint8_t			_integrator_clipping{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
index f78dacaf046..778614e549f 100644
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp
@@ -36,22 +36,24 @@
 
 #include <lib/drivers/device/Device.hpp>
 
+using namespace time_literals;
+using matrix::Vector3f;
+
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
 	ModuleParams(nullptr),
-	_sensor_gyro_pub{ORB_ID(sensor_gyro), priority},
-	_sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority},
+	_sensor_pub{ORB_ID(sensor_gyro), priority},
+	_sensor_control_pub{ORB_ID(sensor_gyro_control), priority},
+	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
+	_sensor_status_pub{ORB_ID(sensor_gyro_status), priority},
+	_device_id{device_id},
 	_rotation{rotation}
 {
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
 
-	_sensor_gyro_pub.get().device_id = device_id;
-	_sensor_gyro_pub.get().scaling = 1.0f;
-	_sensor_gyro_control_pub.get().device_id = device_id;
-
 	// set software low pass filter for controllers
 	updateParams();
-	configure_filter(_param_imu_gyro_cutoff.get());
+	ConfigureFilter(_param_imu_gyro_cutoff.get());
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -70,13 +72,13 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 			gyro_calibration_s cal{};
 			memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal));
 
-			_calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
+			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
 		}
 
 		return PX4_OK;
 
 	case DEVIOCGDEVICEID:
-		return _sensor_gyro_pub.get().device_id;
+		return _device_id;
 
 	default:
 		return -ENOTTY;
@@ -88,67 +90,82 @@ PX4Gyroscope::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
 	union device::Device::DeviceId device_id;
-	device_id.devid = _sensor_gyro_pub.get().device_id;
+	device_id.devid = _device_id;
 
 	// update to new device type
 	device_id.devid_s.devtype = devtype;
 
 	// copy back to report
-	_sensor_gyro_pub.get().device_id = device_id.devid;
-	_sensor_gyro_control_pub.get().device_id = device_id.devid;
+	_device_id = device_id.devid;
 }
 
 void
-PX4Gyroscope::set_sample_rate(unsigned rate)
+PX4Gyroscope::set_sample_rate(uint16_t rate)
 {
 	_sample_rate = rate;
-	_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
+
+	ConfigureFilter(_filter.get_cutoff_freq());
+}
+
+void
+PX4Gyroscope::set_update_rate(uint16_t rate)
+{
+	const uint32_t update_interval = 1000000 / rate;
+
+	_integrator_reset_samples = 4000 / update_interval;
 }
 
 void
 PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 {
-	sensor_gyro_s &report = _sensor_gyro_pub.get();
-	report.timestamp = timestamp;
-
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
 
-	const matrix::Vector3f raw{x, y, z};
+	const Vector3f raw{x, y, z};
 
 	// Apply range scale and the calibrating offset/scale
-	const matrix::Vector3f val_calibrated{((raw * report.scaling) - _calibration_offset)};
+	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
 
 	// Filtered values
-	const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
+	const Vector3f val_filtered{_filter.apply(val_calibrated)};
 
 
-	// publish control data (filtered gyro) immediately
+	// publish control data (filtered) immediately
 	bool publish_control = true;
-	sensor_gyro_control_s &control = _sensor_gyro_control_pub.get();
+	sensor_gyro_control_s control{};
 
 	if (_param_imu_gyro_rate_max.get() > 0) {
 		const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
 
-		if (hrt_elapsed_time(&control.timestamp_sample) < interval) {
+		if (hrt_elapsed_time(&_control_last_publish) < interval) {
 			publish_control = false;
 		}
 	}
 
 	if (publish_control) {
 		control.timestamp_sample = timestamp;
+		control.device_id = _device_id;
 		val_filtered.copyTo(control.xyz);
 		control.timestamp = hrt_absolute_time();
-		_sensor_gyro_control_pub.update();	// publish
+		_sensor_control_pub.publish(control);
+
+		_control_last_publish = control.timestamp_sample;
 	}
 
 
 	// Integrated values
-	matrix::Vector3f integrated_value;
+	Vector3f integrated_value;
 	uint32_t integral_dt = 0;
 
 	if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) {
 
+		sensor_gyro_s report{};
+		report.timestamp = timestamp;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		report.scaling = _scale;
+		report.error_count = _error_count;
+
 		// Raw values (ADC units 0 - 65535)
 		report.x_raw = x;
 		report.y_raw = y;
@@ -163,11 +180,216 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 		report.y_integral = integrated_value(1);
 		report.z_integral = integrated_value(2);
 
-		poll_notify(POLLIN);
-		_sensor_gyro_pub.update();	// publish
+		_sensor_pub.publish(report);
 	}
 }
 
+void
+PX4Gyroscope::updateFIFO(const FIFOSample &sample)
+{
+	// filtered data (control)
+	float x_filtered = _filterArrayX.apply(sample.x, sample.samples);
+	float y_filtered = _filterArrayY.apply(sample.y, sample.samples);
+	float z_filtered = _filterArrayZ.apply(sample.z, sample.samples);
+
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
+
+	const Vector3f raw{x_filtered, y_filtered, z_filtered};
+
+	// Apply range scale and the calibrating offset/scale
+	const Vector3f val_calibrated{(raw * _scale) - _calibration_offset};
+
+
+	// control
+	{
+		// publish control data (filtered) immediately
+		bool publish_control = true;
+		sensor_gyro_control_s control{};
+
+		if (_param_imu_gyro_rate_max.get() > 0) {
+			const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
+
+			if (hrt_elapsed_time(&_control_last_publish) < interval) {
+				publish_control = false;
+			}
+		}
+
+		if (publish_control) {
+			control.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample
+			control.device_id = _device_id;
+			val_calibrated.copyTo(control.xyz);
+			control.timestamp = hrt_absolute_time();
+			_sensor_control_pub.publish(control);
+
+			_control_last_publish = control.timestamp_sample;
+		}
+	}
+
+
+	// status
+	{
+		sensor_gyro_status_s &status = _sensor_status_pub.get();
+
+		const int16_t clip_limit = (_range / _scale) * 0.95f;
+
+		// x clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.x[n]) > clip_limit) {
+				status.clipping[0]++;
+				_integrator_clipping++;
+			}
+		}
+
+		// y clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.y[n]) > clip_limit) {
+				status.clipping[1]++;
+				_integrator_clipping++;
+			}
+		}
+
+		// z clipping
+		for (int n = 0; n < sample.samples; n++) {
+			if (abs(sample.z[n]) > clip_limit) {
+				status.clipping[2]++;
+				_integrator_clipping++;
+			}
+		}
+
+		status.device_id = _device_id;
+		status.error_count = _error_count;
+		status.full_scale_range = _range;
+		status.rotation = _rotation;
+		status.measure_rate = _update_rate;
+		status.sample_rate = _sample_rate;
+		status.temperature = _temperature;
+		status.timestamp = hrt_absolute_time();
+		_sensor_status_pub.publish(status);
+	}
+
+
+	// integrated data (INS)
+	{
+		// reset integrator if previous sample was too long ago
+		if ((sample.timestamp_sample > _timestamp_sample_prev)
+		    && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) {
+
+			ResetIntegrator();
+		}
+
+		if (_integrator_samples == 0) {
+			_integrator_timestamp_sample = sample.timestamp_sample;
+		}
+
+		// integrate
+		_integrator_samples += 1;
+		_integrator_fifo_samples += sample.samples;
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[0] += sample.x[n];
+		}
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[1] += sample.y[n];
+		}
+
+		for (int n = 0; n < sample.samples; n++) {
+			_integrator_accum[2] += sample.z[n];
+		}
+
+		if (_integrator_fifo_samples > 0 && (_integrator_samples >= _integrator_reset_samples)) {
+
+			const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds
+
+			// average integrated values to apply calibration
+			float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples;
+			float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples;
+			float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples;
+
+			// Apply rotation (before scaling)
+			rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg);
+
+			const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg};
+
+			// Apply range scale and the calibrating offset/scale
+			Vector3f val_int_calibrated{(raw_int * _scale) - _calibration_offset};
+			val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f);	// restore
+
+			// publish
+			sensor_gyro_s report{};
+			report.device_id = _device_id;
+			report.temperature = _temperature;
+			report.scaling = _scale;
+			report.error_count = _error_count;
+
+			// Raw values (ADC units 0 - 65535)
+			report.x_raw = sample.x[0];
+			report.y_raw = sample.y[0];
+			report.z_raw = sample.z[0];
+
+			report.x = val_calibrated(0);
+			report.y = val_calibrated(1);
+			report.z = val_calibrated(2);
+
+			report.integral_dt = integrator_dt_us;
+			report.integral_samples = _integrator_fifo_samples;
+			report.x_integral = val_int_calibrated(0);
+			report.y_integral = val_int_calibrated(1);
+			report.z_integral = val_int_calibrated(2);
+			report.integral_clip_count = _integrator_clipping;
+
+			report.timestamp = _integrator_timestamp_sample;
+			_sensor_pub.publish(report);
+
+
+			// reset integrator
+			ResetIntegrator();
+		}
+
+		_timestamp_sample_prev = sample.timestamp_sample;
+	}
+
+	sensor_gyro_fifo_s fifo{};
+
+	fifo.device_id = _device_id;
+	fifo.timestamp_sample = sample.timestamp_sample;
+	fifo.dt = sample.dt;
+	fifo.scale = _scale;
+	fifo.samples = sample.samples;
+
+	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples);
+	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples);
+	memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples);
+
+	fifo.timestamp = hrt_absolute_time();
+	_sensor_fifo_pub.publish(fifo);
+}
+
+void
+PX4Gyroscope::ResetIntegrator()
+{
+	_integrator_samples = 0;
+	_integrator_fifo_samples = 0;
+	_integrator_accum[0] = 0;
+	_integrator_accum[1] = 0;
+	_integrator_accum[2] = 0;
+	_integrator_clipping = 0;
+
+	_integrator_timestamp_sample = 0;
+	_timestamp_sample_prev = 0;
+}
+
+void
+PX4Gyroscope::ConfigureFilter(float cutoff_freq)
+{
+	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
+
+	_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
+	_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
+	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
+}
+
 void
 PX4Gyroscope::print_status()
 {
@@ -178,6 +400,4 @@ PX4Gyroscope::print_status()
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));
 
-	print_message(_sensor_gyro_pub.get());
-	print_message(_sensor_gyro_control_pub.get());
 }
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
index 6999200ff15..171b0f660b1 100644
--- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp
@@ -33,17 +33,19 @@
 
 #pragma once
 
-#include <drivers/device/integrator.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
-#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/drivers/device/integrator.h>
+#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
+#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <px4_platform_common/module_params.h>
-#include <uORB/uORB.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_control.h>
+#include <uORB/topics/sensor_gyro_fifo.h>
+#include <uORB/topics/sensor_gyro_status.h>
 
 class PX4Gyroscope : public cdev::CDev, public ModuleParams
 {
@@ -54,34 +56,82 @@ public:
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
 
-	void set_device_type(uint8_t devtype);
-	void set_error_count(uint64_t error_count) { _sensor_gyro_pub.get().error_count = error_count; }
-	void set_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; }
-	void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; }
+	uint32_t get_device_id() const { return _device_id; }
 
-	void set_sample_rate(unsigned rate);
+	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; }
+	void set_range(float range) { _range = range; }
+	void set_sample_rate(uint16_t rate);
+	void set_scale(float scale) { _scale = scale; }
+	void set_temperature(float temperature) { _temperature = temperature; }
+	void set_update_rate(uint16_t rate);
 
 	void update(hrt_abstime timestamp, float x, float y, float z);
 
 	void print_status();
 
+	struct FIFOSample {
+		hrt_abstime timestamp_sample;
+		uint8_t samples; // number of samples
+		float dt; // in microseconds
+
+		int16_t x[8];
+		int16_t y[8];
+		int16_t z[8];
+	};
+	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");
+	static_assert(sizeof(FIFOSample::z) == sizeof(sensor_gyro_fifo_s::z), "FIFOSample.z invalid size");
+
+	void updateFIFO(const FIFOSample &sample);
+
 private:
 
-	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+	void		ConfigureFilter(float cutoff_freq);
+	void		ResetIntegrator();
 
-	uORB::PublicationMultiData<sensor_gyro_s>		_sensor_gyro_pub;
-	uORB::PublicationMultiData<sensor_gyro_control_s>	_sensor_gyro_control_pub;
+	uORB::PublicationMulti<sensor_gyro_s>			_sensor_pub;		// legacy message
+	uORB::PublicationMulti<sensor_gyro_control_s>		_sensor_control_pub;
+	uORB::PublicationMulti<sensor_gyro_fifo_s>		_sensor_fifo_pub;
+	uORB::PublicationMultiData<sensor_gyro_status_s>	_sensor_status_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
-	Integrator _integrator{4000, true};
 
-	const enum Rotation	_rotation;
+	hrt_abstime	_control_last_publish{0};
+
+	math::LowPassFilter2pArray _filterArrayX{8000, 100};
+	math::LowPassFilter2pArray _filterArrayY{8000, 100};
+	math::LowPassFilter2pArray _filterArrayZ{8000, 100};
+
+	Integrator		_integrator{4000, true};
 
 	matrix::Vector3f	_calibration_offset{0.0f, 0.0f, 0.0f};
 
 	int			_class_device_instance{-1};
 
-	unsigned		_sample_rate{1000};
+
+	uint32_t		_device_id{0};
+
+	const enum Rotation	_rotation;
+
+	float			_range{math::radians(2000.0f)};
+	float			_scale{1.0f};
+	float			_temperature{0.0f};
+
+	uint64_t		_error_count{0};
+
+	uint16_t		_sample_rate{1000};
+	uint16_t		_update_rate{1000};
+
+	// integrator
+	hrt_abstime		_integrator_timestamp_sample{0};
+	hrt_abstime		_timestamp_sample_prev{0};
+	int32_t			_integrator_accum[3] {};
+	uint8_t			_integrator_reset_samples{4};
+	uint8_t			_integrator_samples{0};
+	uint8_t			_integrator_fifo_samples{0};
+	uint8_t			_integrator_clipping{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 6ba96573eb8..7d6c77caaed 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -66,7 +66,7 @@ public:
 	// Reset the filter state to this value
 	float reset(float sample);
 
-private:
+protected:
 
 	float _cutoff_freq{0.0f};
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
new file mode 100644
index 00000000000..7ce89829d05
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file	LowPassFilter2pArray.hpp
+/// @brief	A class to implement a second order low pass filter
+
+#pragma once
+
+#include "LowPassFilter2p.hpp"
+
+namespace math
+{
+
+class LowPassFilter2pArray : public LowPassFilter2p
+{
+public:
+
+	LowPassFilter2pArray(float sample_freq, float cutoff_freq) : LowPassFilter2p(sample_freq, cutoff_freq)
+	{
+	}
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	inline float apply(const int16_t samples[], uint8_t num_samples)
+	{
+		float output = 0.0f;
+
+		for (int n = 0; n < num_samples; n++) {
+			// do the filtering
+			float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+
+			if (n == num_samples - 1) {
+				output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+			}
+
+			_delay_element_2 = _delay_element_1;
+			_delay_element_1 = delay_element_0;
+		}
+
+		// don't allow bad values to propagate via the filter
+		if (!PX4_ISFINITE(output)) {
+			reset(samples[num_samples - 1]);
+			output = samples[num_samples - 1];
+		}
+
+		// return the value. Should be no need to check limits
+		return output;
+	}
+
+};
+
+} // namespace math
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index b1ca8301ee8..55103beeba3 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -553,6 +553,8 @@ void Logger::add_default_topics()
 	add_topic_multi("actuator_outputs", 100);
 	add_topic_multi("battery_status", 500);
 	add_topic_multi("distance_sensor", 100);
+	add_topic_multi("sensor_accel_status", 1000);
+	add_topic_multi("sensor_gyro_status", 1000);
 	add_topic_multi("telemetry_status");
 	add_topic_multi("vehicle_gps_position");
 	add_topic_multi("wind_estimate", 200);