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-05-25 16:17:36 +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
122 changed files with 1799 additions and 1017 deletions
@@ -23,6 +23,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make cubepilot_cubeorange_test'
+                sh 'make cubepilot_cubeorange_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cubepilot_cubeorange_test'
              }
@@ -32,7 +33,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'cubepilot_cubeorange'
              }
@@ -42,6 +43,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'cubepilot_cubeorange_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -91,6 +93,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make cuav_x7pro_test'
+                sh 'make cuav_x7pro_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'cuav_x7pro_test'
              }
@@ -100,7 +103,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'cuav_x7pro'
              }
@@ -110,6 +113,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'cuav_x7pro_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/cuav_x7pro_test/cuav_x7pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -128,7 +132,6 @@ pipeline {
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
                    checkStatus()
                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
                  }
                }
                stage("print topics") {
@@ -158,6 +161,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v3_test'
+                sh 'make px4_fmu-v3_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v3_test'
              }
@@ -167,7 +171,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v3'
              }
@@ -177,6 +181,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'px4_fmu-v3_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v3_test/px4_fmu-v3_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -225,6 +230,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v4_test'
+                sh 'make px4_fmu-v4_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v4_test'
              }
@@ -234,7 +240,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v4'
              }
@@ -244,6 +250,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'px4_fmu-v4_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -290,6 +297,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v4pro_test'
+                sh 'make px4_fmu-v4pro_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v4pro_test'
              }
@@ -299,7 +307,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v4pro'
              }
@@ -309,6 +317,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'px4_fmu-v4pro_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -356,6 +365,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v5_debug'
+                sh 'make px4_fmu-v5_debug bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_debug'
              }
@@ -365,7 +375,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v5'
              }
@@ -374,12 +384,13 @@ pipeline {
                  steps {
                    sh 'export'
                    sh 'find /dev/serial'
+                    unstash 'px4_fmu-v5_debug'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
-                    unstash 'px4_fmu-v5_debug'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -431,6 +442,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v5_optimized'
+                sh 'make px4_fmu-v5_optimized bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_optimized'
              }
@@ -440,7 +452,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v5'
              }
@@ -450,6 +462,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'px4_fmu-v5_optimized'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_optimized/px4_fmu-v5_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_optimized/px4_fmu-v5_optimized.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -497,6 +510,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v5_stackcheck'
+                sh 'make px4_fmu-v5_stackcheck bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_stackcheck'
              }
@@ -506,7 +520,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v5'
              }
@@ -515,12 +529,13 @@ pipeline {
                  steps {
                    sh 'export'
                    sh 'find /dev/serial'
+                    unstash 'px4_fmu-v5_stackcheck'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
-                    unstash 'px4_fmu-v5_stackcheck'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -572,6 +587,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make px4_fmu-v5_test'
+                sh 'make px4_fmu-v5_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'px4_fmu-v5_test'
              }
@@ -581,7 +597,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'px4_fmu-v5'
              }
@@ -591,6 +607,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'px4_fmu-v5_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -638,6 +655,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make modalai_fc-v1_test'
+                sh 'make modalai_fc-v1_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_test'
              }
@@ -647,7 +665,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'modalai_fc-v1'
              }
@@ -657,6 +675,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'modalai_fc-v1_test'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -703,6 +722,7 @@ pipeline {
                sh 'ccache -s'
                sh 'git fetch --tags'
                sh 'make nxp_fmuk66-v3_test'
+                //sh 'make nxp_fmuk66-v3_test bootloader_elf'
                sh 'ccache -s'
                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'nxp_fmuk66-v3_test'
              }
@@ -712,7 +732,7 @@ pipeline {
                }
              }
            } // stage build
-            stage("test") {
+            stage("hardware") {
              agent {
                label 'nxp_fmuk66-v3'
              }
@@ -722,6 +742,7 @@ pipeline {
                    sh 'export'
                    sh 'find /dev/serial'
                    unstash 'nxp_fmuk66-v3_test'
+                    //sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_bootloader.elf'
                    // flash board and watch bootup
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
                  }
@@ -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
@@ -74,6 +74,10 @@ def do_nsh_cmd(port, baudrate, cmd):

        if cmd in serial_line:
            break
+        elif serial_line.startswith(success_cmd) and len(serial_line) <= len(success_cmd) + 2:
+            print_line(serial_line)
+            # we missed the echo, but command ran and succeeded
+            sys.exit(0)
        else:
            if len(serial_line) > 0:
                print_line(serial_line)
@@ -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
@@ -98,7 +98,7 @@ px4_add_board(
 		#vmount
 		#vtol_att_control
 	SYSTEMCMDS
-		#bl_update
+		bl_update
 		dmesg
 		#dumpfile
 		#esc_calib
@@ -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
@@ -156,9 +156,9 @@ if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external"))
 		COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_BOARD}.bin
 		DEPENDS ${PX4_BINARY_OUTPUT}
 		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-)
+	)
 else()
-set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin)
+	set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_BOARD}.bin)
 endif()

 add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT}
@@ -83,18 +83,34 @@ if(Ozone_PATH)
 endif()

 if(bootloader_bin OR (EXISTS "${PX4_BOARD_DIR}/bootloader/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin"))
-	# jlink_flash_bootloader
+
+	if(bootloader_bin)
+		set(BOARD_BL_FIRMWARE_BIN ${bootloader_bin})
+	else()
+		set(BOARD_BL_FIRMWARE_BIN ${PX4_BOARD_DIR}/bootloader/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin)
+	endif()
+
+	# jlink_upload_bootloader
+	if(JLinkGDBServerCLExe_PATH)
+		add_custom_target(jlink_upload_bootloader
+			COMMAND ${PX4_BINARY_DIR}/jlink_gdb_start.sh
+			COMMAND ${CMAKE_CURRENT_SOURCE_DIR}/Debug/upload_jlink_gdb.sh ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf
+			DEPENDS
+				${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.elf
+				${PX4_BINARY_DIR}/jlink_gdb_start.sh
+				${CMAKE_CURRENT_SOURCE_DIR}/Debug/upload_jlink_gdb.sh
+			WORKING_DIRECTORY ${PX4_BINARY_DIR}
+			USES_TERMINAL
+		)
+	endif()
+
+	# jlink_flash_bootloader_bin
 	find_program(JLinkExe_PATH JLinkExe)
 	if(JLinkExe_PATH)
-
-		if(bootloader_bin)
-			set(BOARD_BL_FIRMWARE_BIN ${bootloader_bin})
-		else()
-			set(BOARD_BL_FIRMWARE_BIN ${PX4_BOARD_DIR}/bootloader/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin)
-		endif()
+		file(RELATIVE_PATH BOARD_BL_FIRMWARE_BIN ${PX4_BINARY_DIR} ${BOARD_BL_FIRMWARE_BIN})

 		configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/flash_bootloader.jlink.in ${PX4_BINARY_DIR}/flash_bootloader.jlink @ONLY)
-		add_custom_target(jlink_flash_bootloader
+		add_custom_target(jlink_flash_bootloader_bin
 			COMMAND ${JLinkExe_PATH} -CommandFile ${PX4_BINARY_DIR}/flash_bootloader.jlink
 			DEPENDS
 				px4
@@ -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};
@@ -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
@@ -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)
@@ -1161,6 +1161,7 @@ void Ekf::controlHeightFusion()
 		break;
 	}

+	updateBaroHgtBias();
 	updateBaroHgtOffset();

 	if (_control_status.flags.rng_hgt
@@ -1184,7 +1185,8 @@ void Ekf::controlHeightFusion()
 			Vector3f baro_hgt_obs_var;

 			// 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;
+			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
@@ -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:

@@ -500,6 +502,8 @@ private:
 	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
 	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)
@@ -890,6 +894,7 @@ private:
 	void startGpsHgtFusion();

 	void updateBaroHgtOffset();
+	void updateBaroHgtBias();

 	// return an estimation of the GPS altitude variance
 	float getGpsHeightVariance();
@@ -983,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
--- 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
Daniel Agar	10f4fc7783	ekf2: don't print distance_sensor selection unless there are multiple options	2021-07-19 12:17:12 -04:00
Daniel Agar	1a222bf06e	Tools/HIL/run_nsh_cmd.py handle missing echo, but cmd succeeded	2021-07-19 11:53:54 -04:00
Daniel Agar	87610957a4	flight_mode_manager: pass deltas on estimator reset	2021-07-19 09:34:07 -04:00
Daniel Agar	d79eea0c41	Jenkins: HIL flash bootloaders - add new jlink_upload_bootloader helper target	2021-07-18 23:33:00 -04:00
Daniel Agar	4c59997ff4	Jenkins: HIL attempt to set certain parameters initially - attempt to minimize issues on heavily loaded debug and stackcheck builds - disable systemcmds/bl_update on debug and stackcheck fmu-v5	2021-07-18 21:25:00 -04:00