github actions add timeout-minutes

Fix template_module header
- SubscriptionInterval.hpp includes Subscription.hpp, but not vice versa - The time literal 1_s is defined in the namespace time_literals
2026-06-05 02:30:05 +08:00 · 2021-07-21 12:51:00 -04:00 · 2021-07-21 09:44:09 -04:00 · 2021-07-21 10:39:14 +02:00 · 2021-07-21 02:19:11 -04:00 · 2021-07-20 22:02:03 -07:00
113 changed files with 1984 additions and 1396 deletions
@@ -11,6 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    timeout-minutes: 60
    strategy:
      fail-fast: false
      matrix:
@@ -12,6 +12,7 @@ jobs:
  build:
    runs-on: ubuntu-latest
    container: px4io/px4-dev-clang:2021-05-04
+    timeout-minutes: 60
    steps:
    - uses: actions/checkout@v1
      with:
@@ -12,6 +12,7 @@ jobs:
  build:
    runs-on: ubuntu-latest
    container: px4io/px4-dev-armhf:2021-05-04
+    timeout-minutes: 60
    strategy:
      matrix:
        config: [
@@ -12,6 +12,7 @@ jobs:
  build:
    runs-on: ubuntu-latest
    container: px4io/px4-dev-aarch64:2021-05-04
+    timeout-minutes: 60
    strategy:
      matrix:
        config: [
@@ -11,6 +11,7 @@ on:
 jobs:
  build:
    runs-on: macos-10.15
+    timeout-minutes: 60
    strategy:
      matrix:
        config: [
@@ -12,6 +12,7 @@ jobs:
  build:
    runs-on: ubuntu-latest
    container: px4io/px4-dev-nuttx-focal:2021-05-04
+    timeout-minutes: 60
    strategy:
      matrix:
        config: [
@@ -12,6 +12,7 @@ jobs:
  build:
    runs-on: ubuntu-latest
    container: px4io/px4-dev-nuttx-focal:2021-05-04
+    timeout-minutes: 60
    strategy:
      matrix:
        config: [
@@ -11,6 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    timeout-minutes: 60
    strategy:
      fail-fast: false
      matrix:
@@ -63,3 +63,6 @@
 [submodule "src/lib/events/libevents"]
 	path = src/lib/events/libevents
 	url = https://github.com/mavlink/libevents.git
+[submodule "Tools/simulation-ignition"]
+	path = Tools/simulation-ignition
+	url = https://github.com/Auterion/px4-simulation-ignition.git
@@ -0,0 +1,23 @@
+#!/bin/bash
+#
+# Setup environment to make PX4 visible to Gazebo.
+#
+# Note, this is not necessary if using a ROS catkin workspace with the px4
+# package as the paths are exported.
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+if [ "$#" != 2 ]; then
+    echo -e "usage: source setup_gazebo.bash src_dir build_dir\n"
+    return 1
+fi
+
+SRC_DIR=$1
+BUILD_DIR=$2
+
+# setup Gazebo env and update package path
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
+export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$IGN_GAZEBO_SYSTEM_PLUGIN_PATH:${SRC_DIR}/build/px4_sitl_default/build_ign_gazebo
+export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:${SRC_DIR}/Tools/simulation-ignition/models
+
+echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"
@@ -177,6 +177,10 @@ elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then
 		echo "You need to have gazebo simulator installed!"
 		exit 1
 	fi
+elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
+	echo "Ignition Gazebo"
+	source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
+	ign gazebo -r "${src_path}/Tools/simulation-ignition/worlds/ignition.world"&
 elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	echo "FG setup"
 	cd "${src_path}/Tools/flightgear_bridge/"
@@ -105,6 +105,7 @@

 /* HEATER */
 #define GPIO_HEATER_OUTPUT   /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define HEATER_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS   14
@@ -105,6 +105,7 @@

 /* HEATER */
 #define GPIO_HEATER_OUTPUT   /* PA8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)
+#define HEATER_OUTPUT_EN(on_true)      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM */
 #define DIRECT_PWM_OUTPUT_CHANNELS   14
@@ -117,8 +118,8 @@
 #define GPIO_nPOWER_IN_CAN              /* PG2  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN2)
 #define GPIO_nPOWER_IN_C                /* PG0  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN0)

-#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_CAN /* Brick 1 is Chosen */
-#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_ADC /* Brick 2 is Chosen  */
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_ADC /* Brick 1 is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_CAN /* Brick 2 is Chosen  */
 #define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C   /* USB     is Chosen */

 #define GPIO_VDD_5V_HIPOWER_EN          /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
@@ -174,6 +174,7 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -209,6 +209,7 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -80,6 +80,7 @@ CONFIG_KINETIS_I2C0=y
 CONFIG_KINETIS_I2C1=y
 CONFIG_KINETIS_LPTMR0=y
 CONFIG_KINETIS_LPUART0=y
+CONFIG_KINETIS_LPUART0_RXDMA=y
 CONFIG_KINETIS_MERGE_TTY=y
 CONFIG_KINETIS_PDB=y
 CONFIG_KINETIS_PIT=y
@@ -81,6 +81,7 @@ CONFIG_KINETIS_I2C0=y
 CONFIG_KINETIS_I2C1=y
 CONFIG_KINETIS_LPTMR0=y
 CONFIG_KINETIS_LPUART0=y
+CONFIG_KINETIS_LPUART0_RXDMA=y
 CONFIG_KINETIS_MERGE_TTY=y
 CONFIG_KINETIS_PDB=y
 CONFIG_KINETIS_PIT=y
@@ -64,8 +64,7 @@

 #include <kinetis.h>
 #include <kinetis_uart.h>
-#include <hardware/kinetis_uart.h>
-#include <hardware/kinetis_sim.h>
+#include <kinetis_lpuart.h>
 #include "board_config.h"

 #include "arm_arch.h"
@@ -189,6 +188,26 @@ kinetis_boardinitialize(void)

 	VDD_3V3_SPEKTRUM_POWER_EN(true);
 }
+/****************************************************************************
+ * Name: kinetis_serial_dma_poll_all
+ *
+ * Description:
+ *   Checks receive DMA buffers for received bytes that have not accumulated
+ *   to the point where the DMA half/full interrupt has triggered.
+ *
+ *   This function should be called from a timer or other periodic context.
+ *
+ ****************************************************************************/
+
+void kinetis_lpserial_dma_poll_all(void)
+{
+#if defined(LPSERIAL_HAVE_DMA)
+	kinetis_lpserial_dma_poll();
+#endif
+#if defined(SERIAL_HAVE_DMA)
+	kinetis_serial_dma_poll();
+#endif
+}

 /****************************************************************************
 * Name: board_app_initialize
@@ -235,6 +254,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}

+	/* set up the serial DMA polling */
+#if defined(SERIAL_HAVE_DMA) || defined(LPSERIAL_HAVE_DMA)
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)kinetis_lpserial_dma_poll_all,
+		       NULL);
+#endif
+
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);
@@ -81,6 +81,7 @@ CONFIG_KINETIS_I2C0=y
 CONFIG_KINETIS_I2C1=y
 CONFIG_KINETIS_LPTMR0=y
 CONFIG_KINETIS_LPUART0=y
+CONFIG_KINETIS_LPUART0_RXDMA=y
 CONFIG_KINETIS_MERGE_TTY=y
 CONFIG_KINETIS_PDB=y
 CONFIG_KINETIS_PIT=y
@@ -82,6 +82,7 @@ CONFIG_KINETIS_I2C0=y
 CONFIG_KINETIS_I2C1=y
 CONFIG_KINETIS_LPTMR0=y
 CONFIG_KINETIS_LPUART0=y
+CONFIG_KINETIS_LPUART0_RXDMA=y
 CONFIG_KINETIS_MERGE_TTY=y
 CONFIG_KINETIS_PDB=y
 CONFIG_KINETIS_PIT=y
@@ -64,8 +64,7 @@

 #include <kinetis.h>
 #include <kinetis_uart.h>
-#include <hardware/kinetis_uart.h>
-#include <hardware/kinetis_sim.h>
+#include <kinetis_lpuart.h>
 #include "board_config.h"

 #include "arm_arch.h"
@@ -189,6 +188,26 @@ kinetis_boardinitialize(void)

 	VDD_3V3_SPEKTRUM_POWER_EN(true);
 }
+/****************************************************************************
+ * Name: kinetis_serial_dma_poll_all
+ *
+ * Description:
+ *   Checks receive DMA buffers for received bytes that have not accumulated
+ *   to the point where the DMA half/full interrupt has triggered.
+ *
+ *   This function should be called from a timer or other periodic context.
+ *
+ ****************************************************************************/
+
+void kinetis_lpserial_dma_poll_all(void)
+{
+#if defined(LPSERIAL_HAVE_DMA)
+	kinetis_lpserial_dma_poll();
+#endif
+#if defined(SERIAL_HAVE_DMA)
+	kinetis_serial_dma_poll();
+#endif
+}

 /****************************************************************************
 * Name: board_app_initialize
@@ -235,6 +254,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "DMA alloc FAILED\n");
 	}

+	/* set up the serial DMA polling */
+#if defined(SERIAL_HAVE_DMA) || defined(LPSERIAL_HAVE_DMA)
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)kinetis_lpserial_dma_poll_all,
+		       NULL);
+#endif
+
 	/* initial LED state */
 	drv_led_start();
 	led_off(LED_RED);
@@ -215,6 +215,7 @@
 */
 #define HEATER_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
 #define GPIO_HEATER_OUTPUT   /* GPIO_B1_09 QTIMER2_TIMER3 GPIO2_IO25 */ (GPIO_QTIMER2_TIMER3_1 | HEATER_IOMUX)
+#define HEATER_OUTPUT_EN(on_true)	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -147,6 +147,7 @@
 /* Heater pins */
 #define GPIO_HEATER_INPUT            (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6)
 #define GPIO_HEATER_OUTPUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
+#define HEATER_OUTPUT_EN(on_true)    px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* Power switch controls */

@@ -42,13 +42,13 @@ px4_add_board(
 		#lights/rgbled_pwm
 		#magnetometer # all available magnetometer drivers
 		magnetometer/isentek/ist8310
-		optical_flow # all available optical flow drivers
+		#optical_flow # all available optical flow drivers
 		#osd
 		#pca9685
 		#pca9685_pwm_out
 		#power_monitor/ina226
 		#protocol_splitter
-		pwm_input
+		#pwm_input
 		pwm_out_sim
 		pwm_out
 		px4io
@@ -209,6 +209,8 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PA7  T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
+#define HEATER_OUTPUT_EN(on_true)              px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+

 /* PWM Capture
 *
@@ -202,6 +202,7 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -189,6 +189,7 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PA2  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define HEATER_OUTPUT_EN(on_true)	      px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -227,6 +227,7 @@
 * PWM in future
 */
 #define GPIO_HEATER_OUTPUT   /* PB10  T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* PWM Capture
 *
@@ -145,6 +145,7 @@
 /* Heater pins (reserved) */
 #define GPIO_HEATER_INPUT            (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN6)
 #define GPIO_HEATER_OUTPUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN6)
+#define HEATER_OUTPUT_EN(on_true)    px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))

 /* Power switch controls */

@@ -44,6 +44,7 @@ set(msg_files
 	airspeed.msg
 	airspeed_validated.msg
 	airspeed_wind.msg
+	baro_bias_estimate.msg
 	battery_status.msg
 	camera_capture.msg
 	camera_trigger.msg
@@ -0,0 +1,10 @@
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
+
+uint32 baro_device_id		# unique device ID for the sensor that does not change between power cycles
+float32 bias			# estimated barometric altitude bias (m)
+float32 bias_var		# estimated barometric altitude bias variance (m^2)
+
+float32 innov			# innovation of the last measurement fusion (m)
+float32 innov_var		# innovation variance of the last measurement fusion (m^2)
+float32 innov_test_ratio	# normalized innovation squared test ratio
@@ -331,16 +331,16 @@ int UART_node::init()
 	}

 	// Set up the UART for non-canonical binary communication: 8 bits, 1 stop bit, no parity.
-	uart_config.c_iflag &= !(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF);
+	uart_config.c_iflag &= ~(INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXANY | IXOFF);
 	uart_config.c_iflag |= IGNBRK | IGNPAR;

-	uart_config.c_oflag &= !(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY);
+	uart_config.c_oflag &= ~(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL | NLDLY | VTDLY);
 	uart_config.c_oflag |= NL0 | VT0;

-	uart_config.c_cflag &= !(CSIZE | CSTOPB | PARENB);
+	uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB);
 	uart_config.c_cflag |= CS8 | CREAD | CLOCAL;

-	uart_config.c_lflag &= !(ISIG | ICANON | ECHO | TOSTOP | IEXTEN);
+	uart_config.c_lflag &= ~(ISIG | ICANON | ECHO | TOSTOP | IEXTEN);

 	// Flow control
 	if (hw_flow_control) {
@@ -343,6 +343,8 @@ rtps:
    id: 159
  - msg: event
    id: 160
+  - msg: baro_bias_estimate
+    id: 161
  ########## multi topics: begin ##########
  - msg: actuator_controls_0
    id: 180
@@ -48,6 +48,20 @@ ExternalProject_Add(sitl_gazebo
 	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${build_cores}
 )

+px4_add_git_submodule(TARGET git_ign_gazebo PATH "${PX4_SOURCE_DIR}/Tools/simulation-ignition")
+ExternalProject_Add(simulation-ignition
+	SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation-ignition
+	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+	BINARY_DIR ${PX4_BINARY_DIR}/build_ign_gazebo
+	INSTALL_COMMAND ""
+	DEPENDS git_ign_gazebo
+	USES_TERMINAL_CONFIGURE true
+	USES_TERMINAL_BUILD true
+	EXCLUDE_FROM_ALL true
+	BUILD_ALWAYS 1
+	BUILD_COMMAND ${CMAKE_COMMAND} --build <BINARY_DIR> -- -j ${build_cores}
+)
+
 ExternalProject_Add(mavsdk_tests
 	SOURCE_DIR ${PX4_SOURCE_DIR}/test/mavsdk_tests
 	CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
@@ -90,6 +104,7 @@ set(viewers
 	none
 	jmavsim
 	gazebo
+	ignition
 )

 set(debuggers
@@ -179,6 +194,8 @@ foreach(viewer ${viewers})
 						add_dependencies(${_targ_name} px4 sitl_gazebo)
 					elseif(viewer STREQUAL "jmavsim")
 						add_dependencies(${_targ_name} px4 git_jmavsim)
+					elseif(viewer STREQUAL "ignition")
+						add_dependencies(${_targ_name} px4 simulation-ignition)
 					endif()
 				else()
 					if(viewer STREQUAL "gazebo")
@@ -159,7 +159,7 @@ PARAM_DEFINE_INT32(TRIG_PINS, 56);
 *
 *
 * @min 0
- * @max 4294967040
+ * @max 2147483647
 * @decimal 0
 * @reboot_required true
 * @group Camera trigger
@@ -136,7 +136,7 @@ void Heater::heater_off()
 #endif

 #ifdef HEATER_GPIO
-	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
+	HEATER_OUTPUT_EN(false);
 #endif
 }

@@ -151,7 +151,7 @@ void Heater::heater_on()
 #endif

 #ifdef HEATER_GPIO
-	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1);
+	HEATER_OUTPUT_EN(true);
 #endif
 }

@@ -104,6 +104,9 @@ ADIS16448::ADIS16448(const I2CSPIDriverConfig &config) :
 	_px4_gyro(get_device_id(), config.rotation),
 	_px4_mag(get_device_id(), config.rotation)
 {
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
 }

 ADIS16448::~ADIS16448()
@@ -112,6 +115,7 @@ ADIS16448::~ADIS16448()
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
 	perf_free(_perf_crc_bad);
+	perf_free(_drdy_missed_perf);
 }

 int ADIS16448::init()
@@ -149,6 +153,7 @@ void ADIS16448::print_status()
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_perf_crc_bad);
+	perf_print_counter(_drdy_missed_perf);
 }

 int ADIS16448::probe()
@@ -326,7 +331,19 @@ void ADIS16448::RunImpl()
 		break;

 	case STATE::READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
 				// push backup schedule back
 				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
 			}
@@ -414,8 +431,8 @@ void ADIS16448::RunImpl()
 					}

 					if (imu_updated) {
-						_px4_accel.update(now, accel_x, accel_y, accel_z);
-						_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+						_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);
+						_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);
 					}

 					// DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT
@@ -426,13 +443,13 @@ void ADIS16448::RunImpl()
 						const int16_t mag_x = buffer.XMAGN_OUT;
 						const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT;
 						const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT;
-						_px4_mag.update(now, mag_x, mag_y, mag_z);
+						_px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z);

 						_px4_baro.set_error_count(error_count);
 						_px4_baro.set_temperature(temperature);

 						float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB
-						_px4_baro.update(now, pressure_pa);
+						_px4_baro.update(timestamp_sample, pressure_pa);
 					}

 					success = true;
@@ -105,14 +105,16 @@ private:
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
 	perf_counter_t _perf_crc_bad{nullptr};
+	perf_counter_t _drdy_missed_perf{nullptr};

 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};

-	bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

+	bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ)
 	bool _self_test_passed{false};

 	int16_t _accel_prev[3] {};
@@ -47,12 +47,18 @@ ADIS16470::ADIS16470(const I2CSPIDriverConfig &config) :
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
+	if (_drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
+	}
 }

 ADIS16470::~ADIS16470()
 {
+	perf_free(_reset_perf);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
+	perf_free(_perf_crc_bad);
+	perf_free(_drdy_missed_perf);
 }

 int ADIS16470::init()
@@ -87,9 +93,10 @@ void ADIS16470::print_status()
 	I2CSPIDriverBase::print_status();

 	perf_print_counter(_reset_perf);
-	perf_print_counter(_perf_crc_bad);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
+	perf_print_counter(_perf_crc_bad);
+	perf_print_counter(_drdy_missed_perf);
 }

 int ADIS16470::probe()
@@ -208,7 +215,19 @@ void ADIS16470::RunImpl()
 		break;

 	case STATE::READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < SAMPLE_INTERVAL_US) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
 				// push backup schedule back
 				ScheduleDelayed(SAMPLE_INTERVAL_US * 2);
 			}
@@ -233,7 +252,6 @@ void ADIS16470::RunImpl()
 			static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16470 report not 176 bits");

 			buffer.cmd = static_cast<uint16_t>(Register::GLOB_CMD) << 8;
-
 			set_frequency(SPI_SPEED_BURST);

 			if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) {
@@ -291,7 +309,7 @@ void ADIS16470::RunImpl()
 				accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
 				accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;

-				_px4_accel.update(now, accel_x, accel_y, accel_z);
+				_px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z);


 				int16_t gyro_x = buffer.X_GYRO_OUT;
@@ -301,7 +319,7 @@ void ADIS16470::RunImpl()
 				//  flip y & z to publish right handed with z down (x forward, y right, z down)
 				gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
 				gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
-				_px4_gyro.update(now, gyro_x, gyro_y, gyro_z);
+				_px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z);

 				success = true;

@@ -376,6 +394,7 @@ int ADIS16470::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ADIS16470::DataReady()
 {
+	_drdy_timestamp_sample.store(hrt_absolute_time());
 	ScheduleNow();
 }

@@ -1,35 +1,35 @@
 /****************************************************************************
-*
-*   Copyright (c) 2021 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.
-*
-****************************************************************************/
+ *
+ *   Copyright (c) 2021 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 ADIS16470.hpp
@@ -98,15 +98,18 @@ private:
 	PX4Gyroscope _px4_gyro;

 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
 	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
+	perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))};
+	perf_counter_t _drdy_missed_perf{nullptr};

 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};

+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
+
 	bool _self_test_passed{false};

 	enum class STATE : uint8_t {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -66,7 +66,7 @@ protected:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -74,9 +74,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -162,9 +162,16 @@ void BMI055_Accelerometer::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -192,7 +199,20 @@ void BMI055_Accelerometer::RunImpl()
 					perf_count(_fifo_empty_perf);

 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -242,22 +262,22 @@ void BMI055_Accelerometer::ConfigureAccel()
 	const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);

 	switch (PMU_RANGE) {
-	case range_2g:
+	case range_2g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
 		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
 		break;

-	case range_4g:
+	case range_4g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
 		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
 		break;

-	case range_8g:
+	case range_8g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
 		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
 		break;

-	case range_16g:
+	case range_16g_set:
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
 		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 		break;
@@ -318,11 +338,8 @@ int BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi

 void BMI055_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool BMI055_Accelerometer::DataReadyInterruptConfigure()
@@ -437,7 +454,7 @@ void BMI055_Accelerometer::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);

 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -112,7 +112,7 @@ private:
 	static constexpr uint8_t size_register_cfg{7};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register                    | Set bits, Clear bits
-		{ Register::PMU_RANGE,         PMU_RANGE_BIT::range_16g, Bit1 | Bit0},
+		{ Register::PMU_RANGE,         PMU_RANGE_BIT::range_16g_set, PMU_RANGE_BIT::range_16g_clear},
 		{ Register::ACCD_HBW,          ACCD_HBW_BIT::data_high_bw, 0},
 		{ Register::INT_EN_1,          INT_EN_1_BIT::int_fwm_en, 0},
 		{ Register::INT_MAP_1,         INT_MAP_1_BIT::int1_fwm, 0},
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -162,9 +162,16 @@ void BMI055_Gyroscope::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -192,7 +199,20 @@ void BMI055_Gyroscope::RunImpl()
 					perf_count(_fifo_empty_perf);

 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -316,11 +336,8 @@ int BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a

 void BMI055_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool BMI055_Gyroscope::DataReadyInterruptConfigure()
@@ -434,7 +451,7 @@ void BMI055_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);

 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -97,10 +97,17 @@ enum ACCD_HBW_BIT : uint8_t {
 // PMU_RANGE
 enum PMU_RANGE_BIT : uint8_t {
 	// range<3:0>
-	range_2g  = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
-	range_4g  = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
-	range_8g  = Bit3,        //  ́1000b ́ -> ±8g range
-	range_16g = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
+	range_16g_set   = Bit3 | Bit2, //  ́1100b ́ -> ±16g range
+	range_16g_clear = Bit1 | Bit0,
+
+	range_8g_set    = Bit3,        //  ́1000b ́ -> ±8g range
+	range_8g_clear  = Bit2 | Bit1 | Bit0,
+
+	range_4g_set    = Bit2 | Bit0, //  ́0101b ́ -> ±4g range
+	range_4g_clear  = Bit3 | Bit1,
+
+	range_2g_set    = Bit1 | Bit0, //  ́0011b ́ -> ±2g range
+	range_2g_clear  = Bit3 | Bit2,
 };

 // INT_EN_1
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -66,7 +66,7 @@ protected:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -74,9 +74,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -184,15 +184,19 @@ void BMI088_Accelerometer::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;

 				} else {
-					samples = _fifo_samples;
+					perf_count(_drdy_missed_perf);
 				}

 				// push backup schedule back
@@ -214,6 +218,12 @@ void BMI088_Accelerometer::RunImpl()
 				} else {
 					samples = fifo_byte_counter / sizeof(FIFO::DATA);

+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -226,7 +236,7 @@ void BMI088_Accelerometer::RunImpl()
 			bool success = false;

 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;

 					if (_failure_count > 0) {
@@ -360,11 +370,8 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi

 void BMI088_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@@ -561,7 +568,7 @@ void BMI088_Accelerometer::FIFOReset()
 	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }

 void BMI088_Accelerometer::UpdateTemperature()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -163,9 +163,16 @@ void BMI088_Gyroscope::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -193,7 +200,20 @@ void BMI088_Gyroscope::RunImpl()
 					perf_count(_fifo_empty_perf);

 				} else if (fifo_frame_counter >= 1) {
-					if (FIFORead(now, fifo_frame_counter)) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - FIFO_SAMPLE_DT;
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -317,11 +337,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a

 void BMI088_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@@ -435,7 +452,7 @@ void BMI088_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);

 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{2000}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -72,7 +72,7 @@ protected:
 	int _total_failure_count{0};


-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -81,9 +81,7 @@ protected:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::SELFTEST};
+	} _state{STATE::SELFTEST};

 	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -293,11 +293,7 @@ int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, voi

 void BMI088_Accelerometer::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	ScheduleNow();
 }

 bool BMI088_Accelerometer::DataReadyInterruptConfigure()
@@ -588,7 +584,7 @@ void BMI088_Accelerometer::FIFOReset()
 	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }

 void BMI088_Accelerometer::UpdateTemperature()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -59,7 +59,7 @@ private:
 	static constexpr uint32_t RATE{1600}; // 1600 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -262,11 +262,8 @@ int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *a

 void BMI088_Gyroscope::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool BMI088_Gyroscope::DataReadyInterruptConfigure()
@@ -382,7 +379,7 @@ void BMI088_Gyroscope::FIFOReset()
 	RegisterWrite(Register::FIFO_CONFIG_1, 0);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);

 	// FIFO_CONFIG_0: restore FIFO watermark
 	// FIFO_CONFIG_1: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -58,7 +58,7 @@ private:
 	static constexpr uint32_t RATE{400}; // 2000 Hz
 	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};

-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -237,15 +237,19 @@ void ICM20602::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;

 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}

 				// push backup schedule back
@@ -265,7 +269,13 @@ void ICM20602::RunImpl()

 				} else {
 					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-					samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
+					samples = fifo_count / sizeof(FIFO::DATA);
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}

 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
@@ -279,7 +289,7 @@ void ICM20602::RunImpl()
 			bool success = false;

 			if (samples >= SAMPLES_PER_TRANSFER) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;

 					if (_failure_count > 0) {
@@ -435,11 +445,8 @@ int ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM20602::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool ICM20602::DataReadyInterruptConfigure()
@@ -530,16 +537,14 @@ bool ICM20602::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 	}

 	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);

-	if (fifo_count_bytes >= FIFO::SIZE) {
+	if ((fifo_count_bytes >= FIFO::SIZE) || (fifo_count_samples > FIFO_MAX_SAMPLES)) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
 		return false;
-	}

-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
-
-	if (fifo_count_samples == 0) {
+	} else if (fifo_count_samples == 0) {
 		perf_count(_fifo_empty_perf);
 		return false;
 	}
@@ -571,7 +576,7 @@ void ICM20602::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -140,7 +140,7 @@ private:
 	hrt_abstime _last_config_check_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -151,7 +151,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{24};
@@ -228,9 +228,16 @@ void ICM20608G::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -251,16 +258,21 @@ void ICM20608G::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -402,13 +414,10 @@ int ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM20608G::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -516,8 +525,8 @@ void ICM20608G::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -194,9 +194,16 @@ void ICM20649::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -217,16 +224,21 @@ void ICM20649::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -395,13 +407,10 @@ int ICM20649::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM20649::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -487,10 +496,11 @@ void ICM20649::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits

 uint16_t ICM20649::FIFOReadCount()
 {
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
 	// read FIFO count
 	uint8_t fifo_count_buf[3] {};
 	fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);

 	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -502,9 +512,10 @@ uint16_t ICM20649::FIFOReadCount()

 bool ICM20649::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
+	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
+
 	FIFOTransferBuffer buffer{};
 	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
-	SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);

 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -549,8 +560,8 @@ void ICM20649::FIFOReset()
 	RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 }

 static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -152,8 +152,8 @@ private:

 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -161,12 +161,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{6};
@@ -228,9 +228,16 @@ void ICM20689::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -251,16 +258,21 @@ void ICM20689::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -402,13 +414,10 @@ int ICM20689::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM20689::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -516,8 +525,8 @@ void ICM20689::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -230,9 +230,16 @@ void ICM20948::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -253,16 +260,21 @@ void ICM20948::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -442,13 +454,10 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM20948::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -598,8 +607,8 @@ void ICM20948::FIFOReset()
 	RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);
 }

 static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -73,12 +73,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 9000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -169,8 +169,8 @@ private:

 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -181,7 +181,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{6};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -189,15 +189,19 @@ void ICM40609D::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;

 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}

 				// push backup schedule back
@@ -219,6 +223,12 @@ void ICM40609D::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));

+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -231,7 +241,7 @@ void ICM40609D::RunImpl()
 			bool success = false;

 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;

 					if (_failure_count > 0) {
@@ -409,11 +419,8 @@ int ICM40609D::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM40609D::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool ICM40609D::DataReadyInterruptConfigure()
@@ -580,7 +587,7 @@ void ICM40609D::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }

 void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -146,7 +146,7 @@ private:

 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -154,12 +154,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -190,15 +190,19 @@ void ICM42605::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;

 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}

 				// push backup schedule back
@@ -220,6 +224,12 @@ void ICM42605::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));

+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -232,7 +242,7 @@ void ICM42605::RunImpl()
 			bool success = false;

 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;

 					if (_failure_count > 0) {
@@ -410,11 +420,8 @@ int ICM42605::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM42605::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool ICM42605::DataReadyInterruptConfigure()
@@ -581,7 +588,7 @@ void ICM42605::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }

 void ICM42605::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -146,7 +146,7 @@ private:

 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -154,12 +154,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -190,15 +190,19 @@ void ICM42688P::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;

 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;

 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}

 				// push backup schedule back
@@ -220,6 +224,12 @@ void ICM42688P::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));

+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= FIFO_SAMPLE_DT;
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
@@ -232,7 +242,7 @@ void ICM42688P::RunImpl()
 			bool success = false;

 			if (samples >= 1) {
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;

 					if (_failure_count > 0) {
@@ -368,11 +378,8 @@ int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void ICM42688P::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }

 bool ICM42688P::DataReadyInterruptConfigure()
@@ -553,7 +560,7 @@ void ICM42688P::FIFOReset()
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);

 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }

 static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -75,7 +75,7 @@ private:
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -158,7 +158,7 @@ private:

 	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -169,7 +169,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{13};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -197,9 +197,16 @@ void MPU6000::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -220,7 +227,13 @@ void MPU6000::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
@@ -228,7 +241,7 @@ void MPU6000::RunImpl()
 					perf_count(_fifo_overflow_perf);

 				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -370,13 +383,10 @@ int MPU6000::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void MPU6000::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -488,8 +498,8 @@ void MPU6000::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RESET, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{8};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{8};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -140,8 +140,8 @@ private:
 	FIFO::DATA _fifo_sample_last_new_accel{};
 	uint32_t _fifo_accel_samples_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -149,12 +149,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{7};
@@ -229,9 +229,16 @@ void MPU6500::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -252,16 +259,21 @@ void MPU6500::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -403,13 +415,10 @@ int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void MPU6500::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -521,8 +530,8 @@ void MPU6500::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -138,8 +138,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -150,7 +150,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{15};
@@ -264,9 +264,16 @@ void MPU9250::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -287,16 +294,21 @@ void MPU9250::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -438,13 +450,10 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void MPU9250::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -594,8 +603,8 @@ void MPU9250::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -73,12 +73,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{2};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{2};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 8000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -150,8 +150,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -162,7 +162,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{18};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -197,9 +197,16 @@ void MPU9250_I2C::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
 					perf_count(_drdy_missed_perf);
 				}

@@ -220,16 +227,21 @@ void MPU9250_I2C::RunImpl()

 			} else {
 				// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-				const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
-							SAMPLES_PER_TRANSFER; // round down to nearest
+				uint8_t samples = fifo_count / sizeof(FIFO::DATA);
+
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}

 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

-				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+				} else if (samples >= SAMPLES_PER_TRANSFER) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -371,13 +383,10 @@ int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg)

 void MPU9250_I2C::DataReady()
 {
-	uint32_t expected = 0;
-
 	// at least the required number of samples in the FIFO
-	if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
-	    && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-
-		_drdy_count.store(0);
+	if (++_drdy_count >= _fifo_gyro_samples) {
+		_drdy_timestamp_sample.store(hrt_absolute_time());
+		_drdy_count -= _fifo_gyro_samples;
 		ScheduleNow();
 	}
 }
@@ -495,8 +504,8 @@ void MPU9250_I2C::FIFOReset()
 	RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);

 	// reset while FIFO is disabled
-	_drdy_count.store(0);
-	_drdy_fifo_read_samples.store(0);
+	_drdy_count = 0;
+	_drdy_timestamp_sample.store(0);

 	// FIFO_EN: enable both gyro and accel
 	// USER_CTRL: re-enable FIFO
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -71,12 +71,12 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 1000.f};
-	static constexpr uint32_t SAMPLES_PER_TRANSFER{1};                   // ensure at least 1 new accel sample per transfer
+	static constexpr int32_t SAMPLES_PER_TRANSFER{1};                    // ensure at least 1 new accel sample per transfer
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};             // 1000 Hz gyro
 	static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -136,8 +136,8 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};

-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
-	px4::atomic<uint32_t> _drdy_count{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	int32_t _drdy_count{0};
 	bool _data_ready_interrupt_enabled{false};

 	enum class STATE : uint8_t {
@@ -145,12 +145,10 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{9};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -170,11 +170,13 @@ void LSM9DS1::RunImpl()
 		break;

 	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+
 			// always check current FIFO count
 			bool success = false;
 			// Number of unread words (16-bit axes) stored in FIFO.
 			const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
-			const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
+			uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);

 			if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
 				// overflow
@@ -185,13 +187,19 @@ void LSM9DS1::RunImpl()
 				perf_count(_fifo_empty_perf);

 			} else {
+				// tolerate minor jitter, leave sample to next iteration if behind by only 1
+				if (samples == _fifo_gyro_samples + 1) {
+					timestamp_sample -= FIFO_SAMPLE_DT;
+					samples--;
+				}
+
 				if (samples > FIFO_MAX_SAMPLES) {
 					// not technically an overflow, but more samples than we expected or can publish
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);

 				} else if (samples >= 1) {
-					if (FIFORead(now, samples)) {
+					if (FIFORead(timestamp_sample, samples)) {
 						success = true;

 						if (_failure_count > 0) {
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -74,7 +74,7 @@ private:
 	static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};

 	struct register_config_t {
 		Register reg;
@@ -123,7 +123,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};

 	uint8_t _checked_register{0};
 	static constexpr uint8_t size_register_cfg{6};
@@ -48,7 +48,7 @@
 namespace math
 {

-template<typename T, size_t WINDOW = 3>
+template<typename T, int WINDOW = 3>
 class MedianFilter
 {
 public:
@@ -78,8 +78,6 @@ public:
 		return median();
 	}

-	size_t window_size() const { return WINDOW; }
-
 private:

 	static int cmp(const void *a, const void *b)
@@ -45,6 +45,7 @@ px4_add_module(
 		3500
 	SRCS
 		EKF/airspeed_fusion.cpp
+		EKF/baro_bias_estimator.cpp
 		EKF/control.cpp
 		EKF/covariance.cpp
 		EKF/drag_fusion.cpp
@@ -33,6 +33,7 @@

 add_library(ecl_EKF
 	airspeed_fusion.cpp
+	baro_bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	drag_fusion.cpp
@@ -153,27 +153,6 @@ public:
 		return false;
 	}

-	bool peek_first_older_than(const uint64_t &timestamp, data_type *sample) const
-	{
-		// start looking from newest observation data
-		for (uint8_t i = 0; i < _size; i++) {
-			int index = (_head - i);
-			index = index < 0 ? _size + index : index;
-
-			if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)1e5) {
-				*sample = _buffer[index];
-				return true;
-			}
-
-			if (index == _tail) {
-				// we have reached the tail and haven't got a match
-				return false;
-			}
-		}
-
-		return false;
-	}
-
 	int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }

 private:
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 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 baro_bias_estimator.cpp
+ *
+ * @author Mathieu Bresciani 	<mathieu@auterion.com>
+ */
+
+#include "baro_bias_estimator.hpp"
+
+void BaroBiasEstimator::predict(const float dt)
+{
+	// State is constant
+	// Predict state covariance only
+	_state_var += _process_var * dt * dt;
+	constrainStateVar();
+
+	if (dt > FLT_EPSILON && fabsf(_dt - dt) > 0.001f) {
+		_signed_innov_test_ratio_lpf.setParameters(dt, _lpf_time_constant);
+		_dt = dt;
+	}
+}
+
+void BaroBiasEstimator::constrainStateVar()
+{
+	_state_var = math::constrain(_state_var, 1e-8f, _state_var_max);
+}
+
+void BaroBiasEstimator::fuseBias(const float measurement, const float measurement_var)
+{
+	const float innov_var = _state_var + measurement_var;
+	const float innov = measurement - _state;
+	const float K = _state_var / innov_var;
+	const float innov_test_ratio = computeInnovTestRatio(innov, innov_var);
+
+	if (isTestRatioPassing(innov_test_ratio)) {
+		updateState(K, innov);
+		updateStateCovariance(K);
+
+	}
+
+	if (isLargeOffsetDetected()) {
+		// A bias in the state has been detected by the innovation
+		// sequence check.
+		bumpStateVariance();
+	}
+
+	const float signed_innov_test_ratio = matrix::sign(innov) * innov_test_ratio;
+	_signed_innov_test_ratio_lpf.update(math::constrain(signed_innov_test_ratio, -1.f, 1.f));
+
+	_status = packStatus(innov, innov_var, innov_test_ratio);
+}
+
+inline float BaroBiasEstimator::computeInnovTestRatio(const float innov, const float innov_var) const
+{
+	return innov * innov / (_gate_size * _gate_size * innov_var);
+}
+
+inline bool BaroBiasEstimator::isTestRatioPassing(const float innov_test_ratio) const
+{
+	return innov_test_ratio < 1.f;
+}
+
+inline void BaroBiasEstimator::updateState(const float K, const float innov)
+{
+	_state = math::constrain(_state + K * innov, -_state_max, _state_max);
+}
+
+inline void BaroBiasEstimator::updateStateCovariance(const float K)
+{
+	_state_var -= K * _state_var;
+	constrainStateVar();
+}
+
+inline bool BaroBiasEstimator::isLargeOffsetDetected() const
+{
+	return fabsf(_signed_innov_test_ratio_lpf.getState()) > 0.2f;
+}
+
+inline void BaroBiasEstimator::bumpStateVariance()
+{
+	_state_var += _process_var_boost_gain * _process_var * _dt * _dt;
+}
+
+inline BaroBiasEstimator::status BaroBiasEstimator::packStatus(const float innov, const float innov_var,
+		const float innov_test_ratio) const
+{
+	// Send back status for logging
+	status ret{};
+	ret.bias = _state;
+	ret.bias_var = _state_var;
+	ret.innov = innov;
+	ret.innov_var = innov_var;
+	ret.innov_test_ratio = innov_test_ratio;
+
+	return ret;
+}
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-2021 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 baro_bias_estimator.hpp
+ * @brief Implementation of a single-state baro bias estimator
+ *
+ * state: baro bias (in meters)
+ *
+ * The state is noise driven: Transition matrix A = 1
+ * x[k+1] = Ax[k] + v with v ~ N(0, Q)
+ * y[k] = x[k] + w with w ~ N(0, R)
+ *
+ * The difference between the current barometric altitude and another absolute
+ * reference (e.g.: GNSS altitude) is used as a bias measurement.
+ *
+ * During the measurment update step, the Normalized Innovation Squared (NIS) is checked
+ * and the measurement is rejected if larger than the gate size.
+ *
+ * @author Mathieu Bresciani 	<mathieu@auterion.com>
+ */
+
+#ifndef EKF_BARO_BIAS_ESTIMATOR_HPP
+#define EKF_BARO_BIAS_ESTIMATOR_HPP
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+class BaroBiasEstimator
+{
+public:
+	struct status {
+		float bias;
+		float bias_var;
+		float innov;
+		float innov_var;
+		float innov_test_ratio;
+	};
+
+	BaroBiasEstimator() = default;
+	~BaroBiasEstimator() = default;
+
+	void predict(float dt);
+	void fuseBias(float measurement, float measurement_var);
+
+	void setBias(float bias) { _state = bias; }
+	void setProcessNoiseStdDev(float process_noise)
+	{
+		_process_var = process_noise * process_noise;
+		_state_drift_rate = 3.f * process_noise;
+	}
+	void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
+	void setInnovGate(float gate_size) { _gate_size = gate_size; }
+
+	void setMaxStateNoise(float max_noise) { _state_var_max = max_noise * max_noise; }
+
+	float getBias() const { return _state; }
+	float getBiasVar() const { return _state_var; }
+	const status &getStatus() const { return _status; }
+
+private:
+	float _state{0.f};
+	float _state_max{100.f};
+	float _state_drift_rate{0.005f}; ///< in m/s
+	float _dt{0.01f};
+
+	float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio)
+	float _state_var{0.1f}; ///< Initial state uncertainty variance (m^2)
+	float _process_var{25.0e-6f}; ///< State process noise variance (m^2/s^2)
+	float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2)
+
+	AlphaFilter<float> _signed_innov_test_ratio_lpf; ///< innovation sequence monitoring; used to detect a bias in the state
+
+	status _status;
+
+	void constrainStateVar();
+	float computeKalmanGain(float innov_var) const;
+
+	/*
+	 * Compute the ratio between the Normalized Innovation Squared (NIS)
+	 * and its maximum gate size. Use isTestRatioPassing to know if the
+	 * measurement should be fused or not.
+	 */
+	float computeInnovTestRatio(float innov, float innov_var) const;
+	bool isTestRatioPassing(float innov_test_ratio) const;
+
+	void updateState(float K, float innov);
+	void updateStateCovariance(float K);
+
+	bool isLargeOffsetDetected() const;
+	void bumpStateVariance();
+
+	status packStatus(float innov, float innov_var, float innov_test_ratio) const;
+
+	static constexpr float _lpf_time_constant{10.f}; ///< in seconds
+	static constexpr float _process_var_boost_gain{1.0e3f};
+};
+#endif // !EKF_BARO_BIAS_ESTIMATOR_HPP
@@ -257,6 +257,7 @@ struct parameters {
 	float gps_pos_noise{0.5f};		///< minimum allowed observation noise for gps position fusion (m)
 	float pos_noaid_noise{10.0f};		///< observation noise for non-aiding position fusion (m)
 	float baro_noise{2.0f};			///< observation noise for barometric height fusion (m)
+	float baro_drift_rate{0.005f}; ///< process noise for barometric height bias estimation (m/s)
 	float baro_innov_gate{5.0f};		///< barometric and GPS height innovation consistency gate size (STD)
 	float gps_pos_innov_gate{5.0f};		///< GPS horizontal position innovation consistency gate size (STD)
 	float gps_vel_innov_gate{5.0f};		///< GPS velocity innovation consistency gate size (STD)
@@ -94,18 +94,11 @@ void Ekf::controlFusionModes()

 	// check for arrival of new sensor data at the fusion time horizon
 	_time_prev_gps_us = _gps_sample_delayed.time_us;
-
-	const uint64_t delta_time_prev_gps_us = _gps_sample_delayed.time_us;
 	_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
-
-	// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
-	if (_gps_data_ready && (delta_time_prev_gps_us != 0)) {
-		_delta_time_gps_us = _gps_sample_delayed.time_us - delta_time_prev_gps_us;
-	}
-
 	_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);

 	if (_mag_data_ready) {
+		_mag_lpf.update(_mag_sample_delayed.mag);

 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
@@ -121,25 +114,18 @@ void Ekf::controlFusionModes()
 		}
 	}

-	const uint64_t delta_time_prev_baro_us = _baro_sample_delayed.time_us;
+	_delta_time_baro_us = _baro_sample_delayed.time_us;
 	_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);

 	// if we have a new baro sample save the delta time between this sample and the last sample which is
 	// used below for baro offset calculations
-	if (_baro_data_ready && (delta_time_prev_baro_us != 0)) {
-		_delta_time_baro_us = _baro_sample_delayed.time_us - delta_time_prev_baro_us;
+	if (_baro_data_ready) {
+		_delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us;
 	}

 	{
 		// Get range data from buffer and check validity
-		const uint64_t delta_time_prev_rng_us = _range_sensor.sample().time_us;
 		const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
-
-		// if we have a new sample save the delta time between this sample and the last sample which is used for height offset calculations
-		if (is_rng_data_ready && (delta_time_prev_rng_us != 0)) {
-			_delta_time_rng_us = _range_sensor.sample().time_us - delta_time_prev_rng_us;
-		}
-
 		_range_sensor.setDataReadiness(is_rng_data_ready);

 		// update range sensor angle parameters in case they have changed
@@ -174,14 +160,7 @@ void Ekf::controlFusionModes()
 		_flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps);
 	}

-	const uint64_t delta_time_prev_ev_us = _ev_sample_delayed.time_us;
 	_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
-
-	// if we have a new EV sample save the delta time between this sample and the last sample which is used for height offset calculations
-	if (_ev_data_ready && (delta_time_prev_ev_us != 0)) {
-		_delta_time_ev_us = _ev_sample_delayed.time_us - delta_time_prev_ev_us;
-	}
-
 	_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);

 	// check for height sensor timeouts and reset and change sensor if necessary
@@ -912,7 +891,7 @@ void Ekf::controlHeightSensorTimeouts()

 			// check the baro height source for consistency and freshness
 			const baroSample &baro_init = _baro_buffer.get_newest();
-			const float baro_innov = _state.pos(2) - (_gps_hgt_offset - baro_init.hgt + _baro_hgt_offset);
+			const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
 			const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate);

 			// if baro data is acceptable and GPS data is inaccurate, reset height to baro
@@ -1051,227 +1030,227 @@ void Ekf::checkVerticalAccelerationHealth()

 void Ekf::controlHeightFusion()
 {
-	bool do_range_aid = false;
+	checkRangeAidSuitability();
+	const bool do_range_aid = (_params.range_aid == 1) && isRangeAidSuitable();

-	if (_params.range_aid == 1) {
-		checkRangeAidSuitability();
+	bool fuse_height = false;

-		if (isRangeAidSuitable()) {
-			do_range_aid = true;
+	switch (_params.vdist_sensor_type) {
+	default:
+		ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);
+
+	// FALLTHROUGH
+	case VDIST_SENSOR_BARO:
+		if (do_range_aid && _range_sensor.isDataHealthy()) {
+			setControlRangeHeight();
+			fuse_height = true;
+
+			// we have just switched to using range finder, calculate height sensor offset such that current
+			// measurement matches our current height estimate
+			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				_hgt_sensor_offset = _terrain_vpos;
+			}
+
+		} else if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
+			startBaroHgtFusion();
+			fuse_height = true;
+
+		} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
+			// switch to gps if there was a reset to gps
+			fuse_height = true;
+
+			// we have just switched to using gps height, calculate height sensor offset such that current
+			// measurement matches our current height estimate
+			if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
+				_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+			}
 		}
-	}

-	if (do_range_aid && !_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
-		setControlRangeHeight();
+		break;

-	} else {
-		switch (_params.vdist_sensor_type) {
-		default:
-			ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type);
+	case VDIST_SENSOR_RANGE:
+		if (_range_sensor.isDataHealthy()) {
+			setControlRangeHeight();
+			fuse_height = true;

-		// FALLTHROUGH
-		case VDIST_SENSOR_BARO:
-			if (!do_range_aid && _baro_data_ready && !_baro_hgt_faulty) {
+			if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) {
+				// we have just switched to using range finder, calculate height sensor offset such that current
+				// measurement matches our current height estimate
+				// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
+				if (_control_status.flags.in_air && isTerrainEstimateValid()) {
+					_hgt_sensor_offset = _terrain_vpos;
+
+				} else if (_control_status.flags.in_air) {
+					_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);
+
+				} else {
+					_hgt_sensor_offset = _params.rng_gnd_clearance;
+				}
+			}
+
+		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
+			// fuse baro data if there was a reset to baro
+			fuse_height = true;
+		}
+
+		break;
+
+	case VDIST_SENSOR_GPS:
+
+		// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
+		// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
+		// Do switching between GPS and rangefinder if using range finder as a height source when close
+		// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
+		if (!_control_status_prev.flags.rng_hgt && do_range_aid && _range_sensor.isDataHealthy()) {
+			setControlRangeHeight();
+
+			// we have just switched to using range finder, calculate height sensor offset such that current
+			// measurement matches our current height estimate
+			_hgt_sensor_offset = _terrain_vpos;
+
+		} else if (_control_status_prev.flags.rng_hgt && !do_range_aid) {
+			// must stop using range finder so find another sensor now
+			if (!_gps_hgt_intermittent && _gps_checks_passed) {
+				// GPS quality OK
+				startGpsHgtFusion();
+
+			} else if (!_baro_hgt_faulty) {
+				// Use baro as a fallback
 				startBaroHgtFusion();
 			}

-			break;
-
-		case VDIST_SENSOR_RANGE:
-			if (_range_sensor.isDataHealthy()) {
-				setControlRangeHeight();
-
-			} else if (isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) {
-				// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
-				// and are on the ground, then synthesise a measurement at the expected on ground value
-				_range_sensor.setRange(_params.rng_gnd_clearance);
-				_range_sensor.setDataReadiness(true);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
-
-			break;
-
-		case VDIST_SENSOR_GPS:
-			// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
-			// to pass innovation inconsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
-			// Do switching between GPS and rangefinder if using range finder as a height source when close
-			// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
-			if (_control_status_prev.flags.rng_hgt) {
-				// must stop using range finder so find another sensor now
-				if (!_gps_hgt_intermittent && _gps_checks_passed) {
-					// GPS quality OK
-					startGpsHgtFusion();
-
-				} else if (!_baro_hgt_faulty) {
-					// Use baro as a fallback
-					startBaroHgtFusion();
-				}
-
-			} else if (_control_status.flags.baro_hgt && !_gps_hgt_intermittent && _gps_checks_passed) {
-				// In baro fallback mode and GPS has recovered so start using it
-				startGpsHgtFusion();
-			}
-
-			break;
-
-		case VDIST_SENSOR_EV:
-			// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
-			if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
-				setControlEVHeight();
-			}
-
-			break;
-		}
-	}
-
-
-	if (_baro_data_ready && !_baro_hgt_faulty) {
-		if (!PX4_ISFINITE(_baro_hgt_offset)) {
-			_baro_hgt_offset = _state.pos(2) + _baro_sample_delayed.hgt;
+		} else if (_control_status.flags.baro_hgt && !do_range_aid && !_gps_hgt_intermittent && _gps_checks_passed) {
+			// In baro fallback mode and GPS has recovered so start using it
+			startGpsHgtFusion();
 		}

-		if (!_control_status_prev.flags.baro_hgt && _control_status.flags.baro_hgt) {
-			ECL_INFO("baro height offset %.3f", (double)_baro_hgt_offset);
+		if (_control_status.flags.gps_hgt && _gps_data_ready) {
+			fuse_height = true;
+
+		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+			fuse_height = true;
+
+		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
+			fuse_height = true;
 		}

-		// vertical position innovation - baro measurement has opposite sign to earth z axis
-		_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset;
+		break;

-		// Compensate for positive static pressure transients (negative vertical position innovations)
-		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
-		const float deadzone_start = 0.0f;
-		const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+	case VDIST_SENSOR_EV:

-		if (_control_status.flags.gnd_effect) {
-			if (_baro_hgt_innov(2) < -deadzone_start) {
-				if (_baro_hgt_innov(2) <= -deadzone_end) {
-					_baro_hgt_innov(2) += deadzone_end;
+		// don't start using EV data unless data is arriving frequently, do not reset if pref mode was height
+		if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
+			fuse_height = true;
+			setControlEVHeight();

-				} else {
-					_baro_hgt_innov(2) = -deadzone_start;
-				}
+			if (!_control_status_prev.flags.rng_hgt) {
+				resetHeight();
 			}
 		}

-		if (_control_status.flags.baro_hgt) {
-			// innovation gate size
-			Vector2f baro_hgt_innov_gate{0, fmaxf(_params.baro_innov_gate, 1.f)};
-
-			// observation variance - user parameter defined
-			Vector3f baro_hgt_obs_var{0, 0, sq(fmaxf(_params.baro_noise, 0.01f))};
-
-			// fuse height information
-			fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate, baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
-
-		} else if (!_control_status.flags.baro_hgt && (_delta_time_baro_us != 0)) {
-			// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
-			const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.f, 1.f);
-
-			// apply a 10 second first order low pass filter to height offset
-			_baro_hgt_offset += local_time_step * math::constrain(0.1f * _baro_hgt_innov(2), -0.1f, 0.1f);
-		}
-	}
-
-	if (_gps_data_ready && !_gps_hgt_intermittent) {
-		if (!PX4_ISFINITE(_gps_hgt_offset)) {
-			_gps_hgt_offset = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref;
-		}
-
-		if (!_control_status_prev.flags.gps_hgt && _control_status.flags.gps_hgt) {
-			ECL_INFO("GPS height offset %.3f, Altitude ref: %.3f", (double)_gps_hgt_offset, (double)_gps_alt_ref);
-		}
-
-		// vertical position innovation - gps measurement has opposite sign to earth z axis
-		_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _gps_hgt_offset;
-
-		if (_control_status.flags.gps_hgt) {
-			// innovation gate size
-			Vector2f gps_hgt_innov_gate{0, fmaxf(_params.baro_innov_gate, 1.f)};
-
-			// observation variance
-			Vector3f gps_hgt_obs_var{0, 0, getGpsHeightVariance()};
-
-			// fuse height information
-			fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate, gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
-
-		} else if (!_control_status.flags.gps_hgt && (_delta_time_gps_us != 0)) {
-			// calculate a filtered offset between the GPS origin and local NED origin if we are not using the GPS as a height reference
-			const float local_time_step = math::constrain(1e-6f * _delta_time_gps_us, 0.f, 1.f);
-
-			// apply a 10 second first order low pass filter to height offset
-			_gps_hgt_offset += local_time_step * math::constrain(0.1f * _gps_pos_innov(2), -0.1f, 0.1f);
-		}
-	}
-
-	if (_range_sensor.isDataHealthy()) {
-		if (!PX4_ISFINITE(_rng_hgt_offset)) {
-			// calculate height sensor offset such that current measurement matches our current height estimate
-			// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
-			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-				_rng_hgt_offset = _terrain_vpos;
-
-			} else if (_control_status.flags.in_air) {
-				_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
-
-			} else {
-				_rng_hgt_offset = _params.rng_gnd_clearance;
-			}
-		}
-
-		if (!_control_status_prev.flags.rng_hgt && _control_status.flags.rng_hgt) {
-			ECL_INFO("RNG height offset %.3f", (double)_rng_hgt_offset);
-		}
-
-		// use range finder with tilt correction
-		_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) - _rng_hgt_offset;
-
-		if (_control_status.flags.rng_hgt) {
-			// innovation gate size
-			Vector2f rng_hgt_innov_gate{0, fmaxf(_params.range_innov_gate, 1.f)};
-
-			// observation variance - user parameter defined
-			Vector3f rng_hgt_obs_var{0, 0, fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f)};
-
-			// fuse height information
-			fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate, rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
-
-		} else if (!_control_status.flags.rng_hgt && (_delta_time_rng_us != 0)) {
-			// calculate a filtered offset between the range origin and local NED origin if we are not using the range as a height reference
-			const float local_time_step = math::constrain(1e-6f * _delta_time_rng_us, 0.f, 1.f);
-
-			// apply a 10 second first order low pass filter to height offset
-			_rng_hgt_offset += local_time_step * math::constrain(0.1f * _rng_hgt_innov(2), -0.1f, 0.1f);
-		}
-	}
-
-	if (_ev_data_ready) {
-		if (!PX4_ISFINITE(_ev_hgt_offset)) {
-			_ev_hgt_offset = _state.pos(2) - _ev_sample_delayed.pos(2);
-		}
-
-		if (!_control_status_prev.flags.ev_hgt && _control_status.flags.ev_hgt) {
-			ECL_INFO("EV height offset %.3f", (double)_ev_hgt_offset);
-		}
-
-		// calculate the innovation assuming the external vision observation is in local NED frame
-		_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2) - _ev_hgt_offset;
-
 		if (_control_status.flags.ev_hgt && _ev_data_ready) {
-			// innovation gate size
-			Vector2f ev_hgt_innov_gate{0, fmaxf(_params.ev_pos_innov_gate, 1.f)};
+			fuse_height = true;

-			// observation variance - defined externally
-			Vector3f ev_hgt_obs_var{0, 0, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))};
+		} else if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
+			fuse_height = true;
+
+		} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
+			fuse_height = true;
+		}
+
+		break;
+	}
+
+	updateBaroHgtBias();
+	updateBaroHgtOffset();
+
+	if (_control_status.flags.rng_hgt
+	    && isTimedOut(_time_last_hgt_fuse, 2 * RNG_MAX_INTERVAL)
+	    && !_range_sensor.isDataHealthy()
+	    && _range_sensor.isRegularlySendingData()
+	    && !_control_status.flags.in_air) {
+
+		// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
+		// and are on the ground, then synthesise a measurement at the expected on ground value
+		_range_sensor.setRange(_params.rng_gnd_clearance);
+		_range_sensor.setDataReadiness(true);
+		_range_sensor.setValidity(true); // bypass the checks
+
+		fuse_height = true;
+	}
+
+	if (fuse_height) {
+		if (_control_status.flags.baro_hgt) {
+			Vector2f baro_hgt_innov_gate;
+			Vector3f baro_hgt_obs_var;
+
+			// vertical position innovation - baro measurement has opposite sign to earth z axis
+			const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+			_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
+			// observation variance - user parameter defined
+			baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
+			// innovation gate size
+			baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+
+			// Compensate for positive static pressure transients (negative vertical position innovations)
+			// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
+			const float deadzone_start = 0.0f;
+			const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+
+			if (_control_status.flags.gnd_effect) {
+				if (_baro_hgt_innov(2) < -deadzone_start) {
+					if (_baro_hgt_innov(2) <= -deadzone_end) {
+						_baro_hgt_innov(2) += deadzone_end;
+
+					} else {
+						_baro_hgt_innov(2) = -deadzone_start;
+					}
+				}
+			}

 			// fuse height information
-			fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate, ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
+			fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
+					     baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);

-		} else if (!_control_status.flags.ev_hgt && (_delta_time_ev_us != 0)) {
-			// calculate a filtered offset between the EV height origin and local NED origin if we are not using EV as a height reference
-			const float local_time_step = math::constrain(1e-6f * _delta_time_ev_us, 0.f, 1.f);
+		} else if (_control_status.flags.gps_hgt) {
+			Vector2f gps_hgt_innov_gate;
+			Vector3f gps_hgt_obs_var;
+			// vertical position innovation - gps measurement has opposite sign to earth z axis
+			_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+			gps_hgt_obs_var(2) = getGpsHeightVariance();
+			// innovation gate size
+			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
+					     gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);

-			// apply a 10 second first order low pass filter to height offset
-			_ev_hgt_offset += local_time_step * math::constrain(0.1f * _ev_pos_innov(2), -0.1f, 0.1f);
+		} else if (_control_status.flags.rng_hgt) {
+			Vector2f rng_hgt_innov_gate;
+			Vector3f rng_hgt_obs_var;
+			// use range finder with tilt correction
+			_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
+							     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
+			// observation variance - user parameter defined
+			rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+						   + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
+			// innovation gate size
+			rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
+					     rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
+
+		} else if (_control_status.flags.ev_hgt) {
+			Vector2f ev_hgt_innov_gate;
+			Vector3f ev_hgt_obs_var;
+			// calculate the innovation assuming the external vision observation is in local NED frame
+			_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
+			// observation variance - defined externally
+			ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
+			// innovation gate size
+			ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
+			// fuse height information
+			fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
+					     ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
 		}
 	}
 }
@@ -89,17 +89,16 @@ bool Ekf::update()
 {
 	bool updated = false;

-	// Only run the filter if IMU data in the buffer has been updated
-	if (_imu_updated) {
+	if (!_filter_initialised) {
+		_filter_initialised = initialiseFilter();

 		if (!_filter_initialised) {
-			_filter_initialised = initialiseFilter();
-
-			if (!_filter_initialised) {
-				return false;
-			}
+			return false;
 		}
+	}

+	// Only run the filter if IMU data in the buffer has been updated
+	if (_imu_updated) {
 		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
@@ -116,30 +115,59 @@ bool Ekf::update()
 		runYawEKFGSF();
 	}

-	if (_filter_initialised) {
-		// the output observer always runs
-		// Use full rate IMU data at the current time horizon
-		calculateOutputStates(_newest_high_rate_imu_sample);
-	}
+	// the output observer always runs
+	// Use full rate IMU data at the current time horizon
+	calculateOutputStates(_newest_high_rate_imu_sample);

 	return updated;
 }

 bool Ekf::initialiseFilter()
 {
+	// Filter accel for tilt initialization
+	const imuSample &imu_init = _imu_buffer.get_newest();
+
 	// protect against zero data
-	if (_imu_sample_delayed.delta_vel_dt < 1e-4f || _imu_sample_delayed.delta_ang_dt < 1e-4f) {
+	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
 		return false;
 	}

 	if (_is_first_imu_sample) {
-		_accel_lpf.reset(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
-		_gyro_lpf.reset(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
+		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
 		_is_first_imu_sample = false;

 	} else {
-		_accel_lpf.update(_imu_sample_delayed.delta_vel / _imu_sample_delayed.delta_vel_dt);
-		_gyro_lpf.update(_imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt);
+		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
+	}
+
+	// Sum the magnetometer measurements
+	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+		if (_mag_sample_delayed.time_us != 0) {
+			if (_mag_counter == 0) {
+				_mag_lpf.reset(_mag_sample_delayed.mag);
+
+			} else {
+				_mag_lpf.update(_mag_sample_delayed.mag);
+			}
+
+			_mag_counter++;
+		}
+	}
+
+	// accumulate enough height measurements to be confident in the quality of the data
+	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
+		if (_baro_sample_delayed.time_us != 0) {
+			if (_baro_counter == 0) {
+				_baro_hgt_offset = _baro_sample_delayed.hgt;
+
+			} else {
+				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
+			}
+
+			_baro_counter++;
+		}
 	}

 	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
@@ -149,62 +177,13 @@ bool Ekf::initialiseFilter()
 		}
 	}

-	if (_baro_hgt_counter > 0) {
-		_baro_hgt_offset = _baro_hgt_lpf.getState();
+	if (_baro_counter < _obs_buffer_length) {
+		// not enough baro samples accumulated
+		return false;
 	}

-	if (_gps_hgt_counter > 0) {
-		_gps_hgt_offset = _gps_hgt_lpf.getState();
-	}
-
-	if (_rng_hgt_counter >= _rng_hgt_filter.window_size()) {
-		_rng_hgt_offset = _rng_hgt_filter.median();
-	}
-
-	if (_ev_hgt_counter > 0) {
-		_ev_hgt_offset = _ev_hgt_lpf.getState();
-	}
-
-	switch (_params.vdist_sensor_type) {
-	default:
-
-	// FALLTHROUGH
-	case VDIST_SENSOR_BARO:
-		if (_baro_hgt_counter < _obs_buffer_length) {
-			// not enough samples accumulated
-			return false;
-		}
-
-		setControlBaroHeight();
-		break;
-
-	case VDIST_SENSOR_GPS:
-		if (_gps_hgt_counter < _obs_buffer_length) {
-			// not enough samples accumulated
-			return false;
-		}
-
-		setControlGPSHeight();
-		break;
-
-	case VDIST_SENSOR_RANGE:
-		if (_rng_hgt_counter < _obs_buffer_length) {
-			// not enough samples accumulated
-			return false;
-		}
-
-		setControlRangeHeight();
-		break;
-
-	case VDIST_SENSOR_EV:
-		if (_ev_hgt_counter < _obs_buffer_length) {
-			// not enough samples accumulated
-			return false;
-		}
-
-		setControlEVHeight();
-		break;
-	}
+	// we use baro height initially and switch to GPS/range/EV finder later when it passes checks.
+	setControlBaroHeight();

 	if (!initialiseTilt()) {
 		return false;
@@ -46,6 +46,7 @@
 #include "estimator_interface.h"

 #include "EKFGSF_yaw.h"
+#include "baro_bias_estimator.hpp"

 class Ekf final : public EstimatorInterface
 {
@@ -297,6 +298,7 @@ public:
 	// returns false when data is not available
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+	void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio);

 private:

@@ -378,11 +380,7 @@ private:
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)

 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
-
 	uint64_t _delta_time_baro_us{0};	///< delta time between two consecutive delayed baro samples from the buffer (uSec)
-	uint64_t _delta_time_gps_us{0};
-	uint64_t _delta_time_rng_us{0};
-	uint64_t _delta_time_ev_us{0};

 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s

@@ -495,15 +493,17 @@ private:

 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
+	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
+	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
 	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)

 	// Variables used to perform in flight resets and switch between height sources
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
-	float _baro_hgt_offset{NAN};		///< baro height reading at the local NED origin (m)
-	float _gps_hgt_offset{NAN};
-	float _rng_hgt_offset{NAN};
-	float _ev_hgt_offset{NAN};
+	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
+	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
+	float _baro_hgt_bias{0.0f};
+	float _baro_hgt_bias_var{1.f};

 	// Variables used to control activation of post takeoff functionality
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
@@ -893,6 +893,9 @@ private:
 	void startBaroHgtFusion();
 	void startGpsHgtFusion();

+	void updateBaroHgtOffset();
+	void updateBaroHgtBias();
+
 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();

@@ -985,6 +988,8 @@ private:
 	// yaw estimator instance
 	EKFGSF_yaw _yawEstimator{};

+	BaroBiasEstimator _baro_b_est{};
+
 	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
 	bool _do_ekfgsf_yaw_reset{false};	// true when an emergency yaw reset has been requested
 	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
@@ -255,6 +255,9 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos)
 // Reset height state using the last height measurement
 void Ekf::resetHeight()
 {
+	// Get the most recent GPS data
+	const gpsSample &gps_newest = _gps_buffer.get_newest();
+
 	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {

@@ -262,28 +265,33 @@ void Ekf::resetHeight()
 		if (!_control_status_prev.flags.rng_hgt) {

 			if (_control_status.flags.in_air && isTerrainEstimateValid()) {
-				_rng_hgt_offset = _terrain_vpos;
+				_hgt_sensor_offset = _terrain_vpos;

 			} else if (_control_status.flags.in_air) {
-				_rng_hgt_offset = _range_sensor.getDistBottom() + _state.pos(2);
+				_hgt_sensor_offset = _range_sensor.getDistBottom() + _state.pos(2);

 			} else {
-				_rng_hgt_offset = _params.rng_gnd_clearance;
+				_hgt_sensor_offset = _params.rng_gnd_clearance;
 			}

 		}

 		// update the state and associated variance
-		resetVerticalPositionTo(_rng_hgt_offset - _range_sensor.getDistBottom());
+		resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom());

 		// the state variance is the same as the observation
 		P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));

+		// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
+		const baroSample &baro_newest = _baro_buffer.get_newest();
+		_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
+
 	} else if (_control_status.flags.baro_hgt) {
+		// initialize vertical position with newest baro measurement
+		const baroSample &baro_newest = _baro_buffer.get_newest();

 		if (!_baro_hgt_faulty) {
-			// initialize vertical position with newest baro measurement
-			resetVerticalPositionTo(-_baro_hgt_lpf.getState() + _baro_hgt_offset);
+			resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset);

 			// the state variance is the same as the observation
 			P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
@@ -295,65 +303,39 @@ void Ekf::resetHeight()
 	} else if (_control_status.flags.gps_hgt) {
 		// initialize vertical position and velocity with newest gps measurement
 		if (!_gps_hgt_intermittent) {
-			gpsSample gps_sample;
-
-			if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
-				gps_sample = _gps_buffer.get_newest();
-			}
-
-			resetVerticalPositionTo(_gps_hgt_offset - _gps_hgt_lpf.getState() + _gps_alt_ref);
+			resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref);

 			// the state variance is the same as the observation
-			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_sample.vacc));
+			P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc));
+
+			// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
+			const baroSample &baro_newest = _baro_buffer.get_newest();
+			_baro_hgt_offset = baro_newest.hgt + _state.pos(2);

 		} else {
 			// TODO: reset to last known gps based estimate
 		}

 	} else if (_control_status.flags.ev_hgt) {
-		extVisionSample ext_vision_sample;
+		// initialize vertical position with newest measurement
+		const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();

-		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
-			ext_vision_sample = _ext_vision_buffer.get_newest();
+		// use the most recent data if it's time offset from the fusion time horizon is smaller
+		if (ev_newest.time_us >= _ev_sample_delayed.time_us) {
+			resetVerticalPositionTo(ev_newest.pos(2));
+
+		} else {
+			resetVerticalPositionTo(_ev_sample_delayed.pos(2));
 		}
-
-		resetVerticalPositionTo(_ev_hgt_lpf.getState() + _ev_hgt_offset);
-
-		P.uncorrelateCovarianceSetVariance<1>(9, sq(ext_vision_sample.posVar(2)));
 	}

-	// reset the height sensor offsets which is subtracted from the readings if we need to use it as a backup
-	_baro_hgt_offset = _state.pos(2) + _baro_hgt_lpf.getState();
-	_gps_hgt_offset  = _state.pos(2) + _gps_hgt_lpf.getState() - _gps_alt_ref;
-	_rng_hgt_offset  = _state.pos(2) + _range_sensor.getDistBottom();
-	_ev_hgt_offset   = _state.pos(2) + _ev_hgt_lpf.getState();
-
-
 	// reset the vertical velocity state
 	if (_control_status.flags.gps && !_gps_hgt_intermittent) {
-		gpsSample gps_sample;
-
-		if (!_gps_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &gps_sample)) {
-			gps_sample = _gps_buffer.get_newest();
-		}
-
 		// If we are using GPS, then use it to reset the vertical velocity
-		resetVerticalVelocityTo(gps_sample.vel(2));
+		resetVerticalVelocityTo(gps_newest.vel(2));

 		// the state variance is the same as the observation
-		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
-
-	} else if (_control_status.flags.ev_vel) {
-		extVisionSample ext_vision_sample;
-
-		if (!_ext_vision_buffer.peek_first_older_than(_imu_sample_delayed.time_us, &ext_vision_sample)) {
-			ext_vision_sample = _ext_vision_buffer.get_newest();
-		}
-
-		// use the most recent data if it's time offset from the fusion time horizon is smaller
-		resetVerticalVelocityTo(ext_vision_sample.vel(2));
-
-		P.uncorrelateCovarianceSetVariance<1>(6, math::max(getVisionVelocityVarianceInEkfFrame()(2), sq(0.05f)));
+		P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_newest.sacc));

 	} else {
 		// we don't know what the vertical velocity is, so set it to zero
@@ -1305,6 +1287,10 @@ void Ekf::startBaroHgtFusion()
 {
 	setControlBaroHeight();

+	// We don't need to set a height sensor offset
+	// since we track a separate _baro_hgt_offset
+	_hgt_sensor_offset = 0.0f;
+
 	// Turn off ground effect compensation if it times out
 	if (_control_status.flags.gnd_effect) {
 		if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
@@ -1317,6 +1303,28 @@ void Ekf::startBaroHgtFusion()
 void Ekf::startGpsHgtFusion()
 {
 	setControlGPSHeight();
+
+	// we have just switched to using gps height, calculate height sensor offset such that current
+	// measurement matches our current height estimate
+	if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) {
+		_hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2);
+	}
+}
+
+void Ekf::updateBaroHgtOffset()
+{
+	// calculate a filtered offset between the baro origin and local NED origin if we are not
+	// using the baro as a height reference
+	if (!_control_status.flags.baro_hgt && _baro_data_ready) {
+		const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
+
+		// apply a 10 second first order low pass filter to baro offset
+		const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
+;
+		const float offset_rate_correction =  0.1f * (unbiased_baro + _state.pos(2) -
+								_baro_hgt_offset);
+		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
+	}
 }

 float Ekf::getGpsHeightVariance()
@@ -1329,6 +1337,37 @@ float Ekf::getGpsHeightVariance()
 	return gps_alt_var;
 }

+void Ekf::updateBaroHgtBias()
+{
+	// Baro bias estimation using GPS altitude
+	if (_baro_data_ready) {
+		const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f);
+		_baro_b_est.setMaxStateNoise(_params.baro_noise);
+		_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
+		_baro_b_est.predict(dt);
+	}
+
+	if (_gps_data_ready && !_gps_hgt_intermittent
+	    && _gps_checks_passed &&_NED_origin_initialised
+	    && !_baro_hgt_faulty) {
+		// Use GPS altitude as a reference to compute the baro bias measurement
+		const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
+					- (_gps_sample_delayed.hgt - _gps_alt_ref);
+		const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise);
+		_baro_b_est.fuseBias(baro_bias, baro_bias_var);
+	}
+}
+
+void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio)
+{
+	BaroBiasEstimator::status status = _baro_b_est.getStatus();
+	bias = status.bias;
+	bias_var = status.bias_var;
+	innov = status.innov;
+	innov_var = status.innov_var;
+	innov_test_ratio = status.innov_test_ratio;
+}
+
 Vector3f Ekf::getVisionVelocityInEkfFrame() const
 {
 	Vector3f vel;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	7ccfc91dfc	github actions add timeout-minutes	2021-07-21 12:51:00 -04:00
alessandro	bd17653383	Fix template_module header - SubscriptionInterval.hpp includes Subscription.hpp, but not vice versa - The time literal 1_s is defined in the namespace time_literals	2021-07-21 09:44:09 -04:00
Oleg Evseev	440449b85f	mavlink: use explicit routing for message forwarding Forward message to other mavlink channels only if it is broadcast or the target component was seen on this link before (at least one message was received)	2021-07-21 10:39:14 +02:00
Daniel Agar	b1eb762753	drivers/imu: set sample timestamp from interrupt (if available) - minor scheduling improvements - if expected number of samples larger than expected adjust timestamp sample and save sample for next iteration - when scheduling via data ready interrupt (with no FIFO watermark) count continuously without clearing	2021-07-21 02:19:11 -04:00
honglang	6ac6917430	boards: CUAV-x7pro: reoder brick to fixed cuav hvpm cannot detect voltage	2021-07-20 22:02:03 -07:00
Daniel Agar	949fef1f3b	boards: px4_fmu-v5_optimized disable optical_flow drivers to save flash	2021-07-20 21:43:48 -04:00
David Sidrane	d6ee418cdf	nxp_fmuk66:Add Serial RX DMA on Console	2021-07-20 18:22:35 -04:00
David Sidrane	0709a9bb1c	NuttX With Kinetis LPUART DMA backport	2021-07-20 18:22:35 -04:00
DonLakeFlyer	0af1716864	Fix incorrect max for INT32	2021-07-20 16:29:39 -04:00
David Sidrane	78ecad6170	Revert "nxp_fmuk66-v3:DMA Poll not needed" This reverts commit `962f02220a`.	2021-07-20 07:52:10 -07:00
David Sidrane	89dff2d31c	Revert "nxp_fmuk66-e:DMA Poll not needed" This reverts commit `39d684958d`.	2021-07-20 07:52:10 -07:00
David Sidrane	b85a01f4c2	NuttX with Kinetis SerialPoll back in	2021-07-20 07:52:10 -07:00
Kirill Shilov	e13884410b	heater: added support of inverted IMU heat controller output	2021-07-20 10:37:53 -04:00
TSC21	205d4400eb	add baro_bias_estimate to RTPS list	2021-07-20 04:56:33 -07:00
Oleg	8fbbf56c4d	microRTPS: fix setting UART communication flags bitwise not should be used instead of logical not	2021-07-20 12:34:58 +02:00
PX4 BuildBot	34160bb6c9	Update submodule sitl_gazebo to latest Fri Jul 16 12:39:10 UTC 2021 - sitl_gazebo in PX4/Firmware (`4a0fa08953`): https://github.com/PX4/PX4-SITL_gazebo/commit/1f3f1b1dec5a31c2c400f141fc344f11f68659c9 - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/b6be00542be4d77f436c66ee48c22ca911601a2a - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/1f3f1b1dec5a31c2c400f141fc344f11f68659c9...b6be00542be4d77f436c66ee48c22ca911601a2a b6be005 2021-07-13 Jaeyoung-Lim - Add iris model for control allocation development	2021-07-19 20:19:28 -04:00
JaeyoungLim	58060b23d9	Add ignition gazebo support for PX4 Software-In-The-Loop simulations This commit adds SITL support for ignition gazebo. Ignition gazebo is a replacement for the "classic" gazebo for future applications. The simulation can be run as the following ``` make px4_sitl ignition ```	2021-07-19 19:59:41 -04:00
dagar	a8e3c46cdb	[AUTO COMMIT] update change indication	2021-07-19 16:53:45 -04:00
Daniel Agar	14274ab071	boards: px4_fmu-v5_optimized disable pwm_input to save flash	2021-07-19 16:53:45 -04:00
bresch	54c7e74de3	EKF: add baro bias estimator using GNSS altitude	2021-07-19 16:53:45 -04:00