mavlink: early return in parser

Signed-off-by: Julian Oes <julian@oes.ch>
mavlink: add MAV_SIK_RADIO_ID param back in
2026-06-20 04:20:35 +08:00 · 2023-02-09 21:06:16 +13:00 · 2023-02-09 21:05:44 +13:00 · 2023-02-08 13:15:33 +13:00
156 changed files with 1627 additions and 3299 deletions
@@ -5,12 +5,12 @@
 # @type Quadrotor Wide
 #

-. ${R}etc/init.d-posix/airframes/10016_gazebo-classic_iris
+. ${R}etc/init.d-posix/airframes/10016_iris

-# EKF2: Vision position and heading, no GPS
+# EKF2: Vision position and heading
+param set-default EKF2_AID_MASK 24
 param set-default EKF2_EV_DELAY 5
 param set-default EKF2_EV_CTRL 15
-param set-default EKF2_HGT_REF 3
 param set-default EKF2_GPS_CTRL 0

 # LPE: Vision + baro
@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5

-param set-default NPFG_PERIOD 25
+param set-default FW_L1_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5

-param set-default NPFG_PERIOD 25
+param set-default FW_L1_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,6 +10,8 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

+param set-default FW_L1_PERIOD 15
+
 param set-default FW_P_TC 0.5
 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -20,7 +22,7 @@ param set-default FW_RR_FF 0.20
 param set-default FW_RR_I 0.02
 param set-default FW_RR_P 0.22

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_W_EN 1

@@ -10,7 +10,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5

-param set-default NPFG_PERIOD 25
+param set-default FW_L1_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -10,7 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -11,7 +11,7 @@ param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5

-param set-default NPFG_PERIOD 25
+param set-default FW_L1_PERIOD 25

 param set-default FW_PR_FF 0.40
 param set-default FW_PR_I 0.05
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_ANG 8
 param set-default FW_THR_LND_MAX 0

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_MAN_P_MAX 30

@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -42,7 +42,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2
@@ -46,7 +46,7 @@ param set-default PWM_MAIN_FUNC7 201
 param set-default PWM_MAIN_FUNC8 202
 param set-default PWM_MAIN_FUNC9 203

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -47,7 +47,7 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -11,7 +11,7 @@ param set-default EKF2_MAG_YAWLIM 0
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -15,7 +15,7 @@ param set-default EKF2_MAG_YAWLIM 0

 param set-default FW_LND_ANG 8

-param set-default NPFG_PERIOD 12
+param set-default FW_L1_PERIOD 12

 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.5
@@ -55,6 +55,7 @@ param set-default RWTO_TKOFF 1
 param set-default CA_AIRFRAME 1

 param set-default COM_PREARM_MODE 2
+param set-default CBRK_AIRSPD_CHK 162128

 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
@@ -1,104 +0,0 @@
-#!/bin/sh
-#
-# @name Standard VTOL
-#
-# @type Standard VTOL
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
-
-# TODO: Enable motor failure detection when the
-# VTOL no longer reports 0A for all ESCs in SITL
-param set-default FD_ACT_EN 0
-param set-default FD_ACT_MOT_TOUT 500
-
-param set-default CA_AIRFRAME 2
-
-param set-default COM_PREARM_MODE 2
-
-param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.1515
-param set-default CA_ROTOR0_PY 0.245
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.1515
-param set-default CA_ROTOR1_PY -0.1875
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.1515
-param set-default CA_ROTOR2_PY -0.245
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.1515
-param set-default CA_ROTOR3_PY 0.1875
-param set-default CA_ROTOR3_KM -0.05
-param set-default CA_ROTOR4_AX 1.0
-param set-default CA_ROTOR4_AZ 0.0
-param set-default CA_ROTOR4_PX 0.2
-
-param set-default SIM_GZ_EC_FUNC1 101
-param set-default SIM_GZ_EC_MIN1 10
-param set-default SIM_GZ_EC_MAX1 1500
-param set-default SIM_GZ_EC_FUNC2 102
-param set-default SIM_GZ_EC_MIN2 10
-param set-default SIM_GZ_EC_MAX2 1500
-param set-default SIM_GZ_EC_FUNC3 103
-param set-default SIM_GZ_EC_MIN3 10
-param set-default SIM_GZ_EC_MAX3 1500
-param set-default SIM_GZ_EC_FUNC4 104
-param set-default SIM_GZ_EC_MIN4 10
-param set-default SIM_GZ_EC_MAX4 1500
-
-param set-default SIM_GZ_EC_FUNC5 105
-param set-default SIM_GZ_EC_MIN5 0
-param set-default SIM_GZ_EC_MAX5 3500
-
-param set-default SIM_GZ_SV_FUNC1 201
-param set-default SIM_GZ_SV_FUNC2 202
-param set-default SIM_GZ_SV_FUNC3 203
-
-param set-default COM_RC_IN_MODE 1
-param set-default ASPD_PRIMARY 1
-
-param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS1_TYPE 2
-param set-default CA_SV_CS1_TRQ_R 0.5
-param set-default CA_SV_CS2_TYPE 3
-param set-default CA_SV_CS2_TRQ_P 1.0
-
-param set-default FW_L1_PERIOD 12
-param set-default FW_PR_FF 0.2
-param set-default FW_PR_P 0.9
-param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_FF 0.1
-param set-default FW_RR_P 0.3
-param set-default FW_THR_TRIM 0.25
-param set-default FW_THR_MAX 0.6
-param set-default FW_THR_MIN 0.05
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-
-param set-default MC_AIRMODE 1
-param set-default MC_ROLLRATE_P 0.3
-param set-default MC_YAW_P 1.6
-
-param set-default MIS_TAKEOFF_ALT 10
-
-param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-
-param set-default VT_FWD_THRUST_EN 4
-param set-default VT_F_TRANS_THR 0.75
-param set-default VT_B_TRANS_DUR 8
-param set-default VT_TYPE 2
-param set-default FD_ESCS_EN 0
@@ -73,7 +73,6 @@ px4_add_romfs_files(
 	4001_gz_x500
 	4002_gz_x500_depth
 	4003_gz_rc_cessna
-	4004_gz_standard_vtol

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -80,7 +80,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
-			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -94,7 +93,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
-			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -109,7 +107,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			sensor_baro_sim start
 			sensor_gps_sim start
 			sensor_mag_sim start
-			sensor_airspeed_sim start

 		else
 			echo "ERROR [init] gz_bridge failed to start"
@@ -184,9 +184,6 @@ param set-default SDLOG_DIRS_MAX 7
 param set-default TRIG_INTERFACE 3

 param set-default SYS_FAILURE_EN 1
-# Enable low-battery actions by default for (automated) testing. Battery sim
-# does not go below 50% by default, but failure injection can trigger failsafes.
-param set-default COM_LOW_BAT_ACT 2

 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
@@ -38,7 +38,7 @@ param set-default EKF2_GPS_P_GATE 10
 param set-default EKF2_GPS_V_GATE 10

 param set-default FW_ARSP_MODE 1
-param set-default NPFG_PERIOD 25
+param set-default FW_L1_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18
 param set-default FW_PR_P 0.15
@@ -30,7 +30,7 @@ param set-default BAT1_N_CELLS 6
 param set-default FW_AIRSPD_MAX 30
 param set-default FW_AIRSPD_MIN 19
 param set-default FW_AIRSPD_TRIM 23
-param set-default FW_PN_R_SLEW_MAX 40
+param set-default FW_L1_R_SLEW_MAX 40
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25
@@ -20,7 +20,7 @@ control_allocator start
 #
 fw_rate_control start
 fw_att_control start
-fw_path_navigation start
+fw_pos_control_l1 start
 airspeed_selector start

 #
@@ -37,7 +37,7 @@ fi

 fw_rate_control start vtol
 fw_att_control start vtol
-fw_path_navigation start vtol
+fw_pos_control_l1 start vtol
 fw_autotune_attitude_control start vtol

 # Start Land Detector
@@ -1,15 +0,0 @@
-<?xml version="1.0"?>
-<model>
-  <name>Standard VTOL</name>
-  <version>1.0</version>
-  <sdf version='1.10'>model.sdf</sdf>
-
-  <author>
-   <name>Roman Bapst</name>
-   <email>roman@px4.io</email>
-  </author>
-
-  <description>
-    This is a model of a standard VTOL quad plane.
-  </description>
-</model>
@@ -1,748 +0,0 @@
-<?xml version="1.0"?>
-<!-- DO NOT EDIT: Generated from standard_vtol.sdf.jinja -->
-<sdf version='1.10'>
-  <model name='standard_vtol'>
-    <pose>0 0 0.246 0 0 0</pose>
-    <link name='base_link'>
-      <pose>0 0 0 0 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>5</mass>
-        <inertia>
-          <ixx>0.477708333333</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.341666666667</iyy>
-          <iyz>0</iyz>
-          <izz>0.811041666667</izz>
-        </inertia>
-      </inertial>
-      <collision name='base_link_collision'>
-        <pose>0 0 -0.07 0 0 0</pose>
-        <geometry>
-          <box>
-            <size>0.55 2.144 0.05</size>
-          </box>
-        </geometry>
-        <surface>
-          <contact>
-            <ode>
-              <kp>100000</kp>
-              <kd>1.0</kd>
-              <max_vel>0.1</max_vel>
-              <min_depth>0.001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='base_link_visual'>
-        <pose>0.53 -1.072 -0.1 1.5707963268 0 3.1415926536</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='left_motor_column'>
-        <pose>0 0.35 0.01 0 0 0</pose>
-        <geometry>
-          <box>
-            <size>0.74 0.03 0.03</size>
-          </box>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='right_motor_column'>
-        <pose>0 -0.35 0.01 0 0 0</pose>
-        <geometry>
-          <box>
-            <size>0.74 0.03 0.03</size>
-          </box>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='m0'>
-        <pose>-0.35 0.35 0.045 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.035</length>
-            <radius>0.02</radius>
-          </cylinder>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='m1'>
-        <pose>-0.35 -0.35 0.045 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.035</length>
-            <radius>0.02</radius>
-          </cylinder>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='m2'>
-        <pose>0.35 -0.35 0.045 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.035</length>
-            <radius>0.02</radius>
-          </cylinder>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <visual name='m3'>
-        <pose>0.35 0.35 0.045 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.035</length>
-            <radius>0.02</radius>
-          </cylinder>
-        </geometry>
-        <material>
-          <ambient>.175 .175 .175 1.0</ambient>
-          <diffuse>.175 .175 .175 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-      <sensor name="imu_sensor" type="imu">
-        <always_on>1</always_on>
-        <update_rate>250</update_rate>
-      </sensor>
-    </link>
-    <link name='rotor_0'>
-      <pose>0.35 -0.35 0.07 0 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.005</mass>
-        <inertia>
-          <ixx>9.75e-07</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.000166704</iyy>
-          <iyz>0</iyz>
-          <izz>0.000167604</izz>
-        </inertia>
-      </inertial>
-      <collision name='rotor_0_collision'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.005</length>
-            <radius>0.1</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='rotor_0_visual'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-    </link>
-    <joint name='rotor_0_joint' type='revolute'>
-      <child>rotor_0</child>
-      <parent>base_link</parent>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='rotor_1'>
-      <pose>-0.35 0.35 0.07 0 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.005</mass>
-        <inertia>
-          <ixx>9.75e-07</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.000166704</iyy>
-          <iyz>0</iyz>
-          <izz>0.000167604</izz>
-        </inertia>
-      </inertial>
-      <collision name='rotor_1_collision'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.005</length>
-            <radius>0.1</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='rotor_1_visual'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-    </link>
-    <joint name='rotor_1_joint' type='revolute'>
-      <child>rotor_1</child>
-      <parent>base_link</parent>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='rotor_2'>
-      <pose>0.35 0.35 0.07 0 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.005</mass>
-        <inertia>
-          <ixx>9.75e-07</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.000166704</iyy>
-          <iyz>0</iyz>
-          <izz>0.000167604</izz>
-        </inertia>
-      </inertial>
-      <collision name='rotor_2_collision'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.005</length>
-            <radius>0.1</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='rotor_2_visual'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-    </link>
-    <joint name='rotor_2_joint' type='revolute'>
-      <child>rotor_2</child>
-      <parent>base_link</parent>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='rotor_3'>
-      <pose>-0.35 -0.35 0.07 0 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.005</mass>
-        <inertia>
-          <ixx>9.75e-07</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.000166704</iyy>
-          <iyz>0</iyz>
-          <izz>0.000167604</izz>
-        </inertia>
-      </inertial>
-      <collision name='rotor_3_collision'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.005</length>
-            <radius>0.1</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='rotor_3_visual'>
-        <pose>0 0 0 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-    </link>
-    <joint name='rotor_3_joint' type='revolute'>
-      <child>rotor_3</child>
-      <parent>base_link</parent>
-      <axis>
-        <xyz>0 0 1</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-
-    <link name='rotor_puller'>
-      <pose>-0.22 0 0.0 0 1.57 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.005</mass>
-        <inertia>
-          <ixx>9.75e-07</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.000166704</iyy>
-          <iyz>0</iyz>
-          <izz>0.000167604</izz>
-        </inertia>
-      </inertial>
-      <collision name='rotor_puller_collision'>
-        <pose>0.0 0 -0.04 0 0 0</pose>
-        <geometry>
-          <cylinder>
-            <length>0.005</length>
-            <radius>0.06</radius>
-          </cylinder>
-        </geometry>
-        <surface>
-          <contact>
-            <ode/>
-          </contact>
-          <friction>
-            <ode/>
-          </friction>
-        </surface>
-      </collision>
-      <visual name='rotor_puller_visual'>
-        <pose>0 0 -0.04 0 0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>0.8 0.8 0.8</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-      <gravity>1</gravity>
-      <velocity_decay/>
-      <self_collide>0</self_collide>
-    </link>
-    <joint name='rotor_puller_joint' type='revolute'>
-      <pose>0.0 0 0.0 0 -1.57 0</pose>
-      <child>rotor_puller</child>
-      <parent>base_link</parent>
-      <axis>
-        <xyz>1 0 0</xyz>
-        <limit>
-          <lower>-1e+16</lower>
-          <upper>1e+16</upper>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-
-    <link name="left_elevon">
-      <inertial>
-        <mass>0.00000001</mass>
-        <inertia>
-          <ixx>0.000001</ixx>
-          <ixy>0.0</ixy>
-          <iyy>0.000001</iyy>
-          <ixz>0.0</ixz>
-          <iyz>0.0</iyz>
-          <izz>0.000001</izz>
-        </inertia>
-        <pose>0 0.3 0 0.00 0 0.0</pose>
-      </inertial>
-      <visual name='left_elevon_visual'>
-        <pose>-0.105 0.004 -0.034 1.5707963268 0 3.1415926536</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-    </link>
-    <link name="right_elevon">
-      <inertial>
-        <mass>0.00000001</mass>
-        <inertia>
-          <ixx>0.000001</ixx>
-          <ixy>0.0</ixy>
-          <iyy>0.000001</iyy>
-          <ixz>0.0</ixz>
-          <iyz>0.0</iyz>
-          <izz>0.000001</izz>
-        </inertia>
-        <pose>0 -0.6 0 0.00 0 0.0</pose>
-      </inertial>
-      <visual name='right_elevon_visual'>
-        <pose>0.281 -1.032 -0.034 1.5707963268 0 3.1415926536</pose>
-        <geometry>
-          <mesh>
-            <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
-          </mesh>
-        </geometry>
-        <material>
-          <ambient>0 0 1 1.0</ambient>
-          <diffuse>0 0 1 1.0</diffuse>
-        </material>
-      </visual>
-    </link>
-    <link name="elevator">
-      <inertial>
-        <mass>0.00000001</mass>
-        <inertia>
-          <ixx>0.000001</ixx>
-          <ixy>0.0</ixy>
-          <iyy>0.000001</iyy>
-          <ixz>0.0</ixz>
-          <iyz>0.0</iyz>
-          <izz>0.000001</izz>
-        </inertia>
-        <pose> -0.5 0 0 0.00 0 0.0</pose>
-      </inertial>
-    </link>
-    <joint name='servo_0' type='revolute'>
-      <parent>base_link</parent>
-      <child>left_elevon</child>
-      <pose>-0.18 0.6 -0.005 0 0 0.265</pose>
-      <axis>
-        <xyz>0 1 0</xyz>
-        <limit>
-          <lower>-0.53</lower>
-          <upper>0.53</upper>
-        </limit>
-        <dynamics>
-          <damping>1.000</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-        </ode>
-      </physics>
-    </joint>
-    <joint name='servo_1' type='revolute'>
-      <parent>base_link</parent>
-      <child>right_elevon</child>
-      <pose>-0.18 -0.6 -0.005 0 0 -0.265</pose>
-      <axis>
-        <xyz>0 1 0</xyz>
-        <limit>
-          <lower>-0.53</lower>
-          <upper>0.53</upper>
-        </limit>
-        <dynamics>
-          <damping>1.000</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-        </ode>
-      </physics>
-    </joint>
-    <joint name='servo_2' type='revolute'>
-      <parent>base_link</parent>
-      <child>elevator</child>
-      <pose> -0.5 0 0 0 0 0</pose>
-      <axis>
-        <xyz>0 1 0</xyz>
-        <limit>
-          <!-- -30/+30 deg. -->
-          <lower>-0.53</lower>
-          <upper>0.53</upper>
-        </limit>
-        <dynamics>
-          <damping>1.000</damping>
-        </dynamics>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-        </ode>
-      </physics>
-    </joint>
-    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
-      <a0>0.05984281113</a0>
-      <cla>4.752798721</cla>
-      <cda>0.6417112299</cda>
-      <cma>0.0</cma>
-      <alpha_stall>0.3391428111</alpha_stall>
-      <cla_stall>-3.85</cla_stall>
-      <cda_stall>-0.9233984055</cda_stall>
-      <cma_stall>0</cma_stall>
-      <cp>-0.05 0.3 0.05</cp>
-      <area>0.50</area>
-      <air_density>1.2041</air_density>
-      <forward>1 0 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>base_link</link_name>
-      <control_joint_name>servo_0</control_joint_name>
-      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
-      <joint_name>servo_0</joint_name>
-      <sub_topic>servo_0</sub_topic>
-    </plugin>
-    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
-      <a0>0.05984281113</a0>
-      <cla>4.752798721</cla>
-      <cda>0.6417112299</cda>
-      <cma>0.0</cma>
-      <alpha_stall>0.3391428111</alpha_stall>
-      <cla_stall>-3.85</cla_stall>
-      <cda_stall>-0.9233984055</cda_stall>
-      <cma_stall>0</cma_stall>
-      <cp>-0.05 -0.3 0.05</cp>
-      <area>0.50</area>
-      <air_density>1.2041</air_density>
-      <forward>1 0 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>base_link</link_name>
-      <control_joint_name>servo_1</control_joint_name>
-      <control_joint_rad_to_cl>-1.0</control_joint_rad_to_cl>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
-      <joint_name>servo_1</joint_name>
-      <sub_topic>servo_1</sub_topic>
-    </plugin>
-    <plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
-      <a0>-0.2</a0>
-      <cla>4.752798721</cla>
-      <cda>0.6417112299</cda>
-      <cma>0.0</cma>
-      <alpha_stall>0.3391428111</alpha_stall>
-      <cla_stall>-3.85</cla_stall>
-      <cda_stall>-0.9233984055</cda_stall>
-      <cma_stall>0</cma_stall>
-      <cp>-0.5 0 0</cp>
-      <area>0.01</area>
-      <air_density>1.2041</air_density>
-      <forward>1 0 0</forward>
-      <upward>0 0 1</upward>
-      <link_name>base_link</link_name>
-      <control_joint_name>servo_2</control_joint_name>
-          <control_joint_name>servo_2</control_joint_name>
-      <control_joint_rad_to_cl>-12.0</control_joint_rad_to_cl>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
-      <joint_name>servo_2</joint_name>
-      <sub_topic>servo_2</sub_topic>
-    </plugin>
-    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
-      <jointName>rotor_0_joint</jointName>
-      <linkName>rotor_0</linkName>
-      <turningDirection>ccw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1500</maxRotVelocity>
-      <motorConstant>2e-05</motorConstant>
-      <momentConstant>0.06</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>0</motorNumber>
-      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
-      <jointName>rotor_1_joint</jointName>
-      <linkName>rotor_1</linkName>
-      <turningDirection>ccw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1500</maxRotVelocity>
-      <motorConstant>2e-05</motorConstant>
-      <momentConstant>0.06</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>1</motorNumber>
-      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin
-      filename="gz-sim-multicopter-motor-model-system"
-      name="gz::sim::systems::MulticopterMotorModel">
-      <jointName>rotor_2_joint</jointName>
-      <linkName>rotor_2</linkName>
-      <turningDirection>cw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1500</maxRotVelocity>
-      <motorConstant>2e-05</motorConstant>
-      <momentConstant>0.06</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>2</motorNumber>
-      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin
-      filename="gz-sim-multicopter-motor-model-system"
-      name="gz::sim::systems::MulticopterMotorModel">
-      <jointName>rotor_3_joint</jointName>
-      <linkName>rotor_3</linkName>
-      <turningDirection>cw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>1500</maxRotVelocity>
-      <motorConstant>2e-05</motorConstant>
-      <momentConstant>0.06</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>3</motorNumber>
-      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <plugin
-      filename="gz-sim-multicopter-motor-model-system"
-      name="gz::sim::systems::MulticopterMotorModel">
-      <jointName>rotor_puller_joint</jointName>
-      <linkName>rotor_puller</linkName>
-      <turningDirection>cw</turningDirection>
-      <timeConstantUp>0.0125</timeConstantUp>
-      <timeConstantDown>0.025</timeConstantDown>
-      <maxRotVelocity>3500</maxRotVelocity>
-      <motorConstant>8.54858e-06</motorConstant>
-      <momentConstant>0.01</momentConstant>
-      <commandSubTopic>command/motor_speed</commandSubTopic>
-      <motorNumber>4</motorNumber>
-      <rotorDragCoefficient>0.000106428</rotorDragCoefficient>
-      <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
-      <rotorVelocitySlowdownSim>20</rotorVelocitySlowdownSim>
-      <motorType>velocity</motorType>
-    </plugin>
-    <static>0</static>
-  </model>
-</sdf>
@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,7 +51,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -67,6 +67,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -33,7 +33,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -38,7 +38,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -39,7 +39,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -29,7 +29,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -30,7 +30,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -43,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -31,6 +31,7 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,6 +51,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -96,3 +98,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -32,6 +32,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -43,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -51,6 +52,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -97,3 +99,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -31,6 +31,7 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -42,7 +43,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,6 +51,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -45,7 +45,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,7 +46,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -48,7 +48,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -2,7 +2,6 @@ CONFIG_DRIVERS_ADC_ADS1115=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
 CONFIG_SYSTEMCMDS_MICROBENCH=y
@@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -20,7 +20,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -35,8 +35,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
-# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
-# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_MFT=y
@@ -9,5 +9,5 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
@@ -49,7 +49,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -3,7 +3,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
-CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_TEST_PPM=y
@@ -55,7 +55,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -18,7 +18,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n
 CONFIG_MODULES_ESC_BATTERY=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_PATH_NAVIGATION=n
+CONFIG_MODULES_FW_POS_CONTROL_L1=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
@@ -58,7 +58,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -42,7 +42,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -47,7 +47,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -17,7 +17,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -30,7 +30,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -35,6 +35,7 @@ CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -46,7 +47,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -54,6 +55,7 @@ CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -99,3 +101,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -44,7 +44,7 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_PATH_NAVIGATION=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -126,7 +126,6 @@ set(msg_files
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
-	ModeCompleted.msg
 	NavigatorMissionItem.msg
 	NpfgStatus.msg
 	ObstacleDistance.msg
@@ -1,14 +0,0 @@
-# Mode completion result, published by an active mode.
-# Note that this is not always published (e.g. when a user switches modes or on
-# failsafe activation)
-uint64 timestamp				 # time since system start (microseconds)
-
-
-uint8 RESULT_SUCCESS = 0
-# [1-99]: reserved
-uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error)
-
-uint8 result                     # One of RESULT_*
-
-uint8 nav_state                  # Source mode
-
@@ -84,6 +84,7 @@ uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    		# Request to send a single instanc
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530		# Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531		# Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
+uint16 VEHICLE_CMD_SET_AT_PARAM = 550			# Set AT command for SiK radio
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 	# Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
 uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 	# Gimbal configuration to set which sysid/compid is in primary and secondary control
 uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   	# Start image capture sequence.
@@ -71,7 +71,7 @@ __END_DECLS
 * Messages that should never be filtered or compiled out
 ****************************************************************************/
 #define PX4_INFO(FMT, ...) 	qurt_log(_PX4_LOG_LEVEL_INFO, __FILE__, __LINE__, FMT, ##__VA_ARGS__)
-#define PX4_INFO_RAW(FMT, ...) 	qurt_log_raw(FMT, ##__VA_ARGS__)
+#define PX4_INFO_RAW(FMT, ...) 	__px4_log_omit(_PX4_LOG_LEVEL_INFO, FMT, ##__VA_ARGS__)

 #if defined(TRACE_BUILD)
 /****************************************************************************
@@ -57,14 +57,4 @@ static __inline void qurt_log(int level, const char *file, int line,
 	qurt_log_to_apps(level, buf);
 }

-static __inline void qurt_log_raw(const char *format, ...)
-{
-	char buf[256];
-	va_list args;
-	va_start(args, format);
-	vsnprintf(buf, sizeof(buf), format, args);
-	va_end(args);
-	qurt_log_to_apps(1, buf);
-}
-
 __END_DECLS
@@ -29,7 +29,7 @@ flight_mode_manager start
 mc_pos_control start vtol
 mc_att_control start vtol
 mc_rate_control start vtol
-fw_path_navigation start vtol
+fw_pos_control_l1 start vtol
 fw_att_control start vtol
 airspeed_selector start

@@ -59,7 +59,7 @@ flight_mode_manager status
 mc_pos_control status
 mc_att_control status
 mc_rate_control status
-fw_path_navigation status
+fw_pos_control_l1 status
 fw_att_control status
 airspeed_selector status
 dataman status
@@ -74,7 +74,7 @@ mc_att_control stop
 fw_att_control stop
 flight_mode_manager stop
 mc_pos_control stop
-fw_path_navigation stop
+fw_pos_control_l1 stop
 navigator stop
 commander stop
 land_detector stop
@@ -56,7 +56,7 @@ ekf2 start
 land_detector start fixedwing

 fw_att_control start
-fw_path_navigation start
+fw_pos_control_l1 start

 mavlink start -n SoftAp -x -u 14556 -r 1000000 -p
 # -n name of wifi interface: SoftAp, wlan or your custom interface,
@@ -63,7 +63,7 @@ airspeed_selector start
 land_detector start fixedwing
 flight_mode_manager start
 fw_att_control start
-fw_path_navigation start
+fw_pos_control_l1 start

 mavlink start -x -u 14556 -r 1000000 -p

@@ -41,7 +41,7 @@ navigator start
 ekf2 start
 land_detector start fixedwing
 fw_att_control start
-fw_path_navigation start
+fw_pos_control_l1 start

 mavlink start -x -u 14556 -r 1000000 -p

@@ -73,7 +73,6 @@ protected:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
-		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2023 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
@@ -120,7 +120,6 @@ void BMI088_Accelerometer::RunImpl()
 	case STATE::RESET:
 		// ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
 		RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
-		DataReadyInterruptDisable();
 		_reset_timestamp = now;
 		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
@@ -153,9 +152,21 @@ void BMI088_Accelerometer::RunImpl()

 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(10_ms);
+			// if configure succeeded then start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();

 		} else {
 			// CONFIGURE not complete
@@ -172,25 +183,6 @@ void BMI088_Accelerometer::RunImpl()

 		break;

-	case STATE::FIFO_RESET:
-		// if configure succeeded then start reading from FIFO
-		_state = STATE::FIFO_READ;
-
-		FIFOReset();
-
-		if (DataReadyInterruptConfigure()) {
-			_data_ready_interrupt_enabled = true;
-
-			// backup schedule as a watchdog timeout
-			ScheduleDelayed(100_ms);
-
-		} else {
-			_data_ready_interrupt_enabled = false;
-			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-		}
-
-		break;
-
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2023 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
@@ -102,7 +102,6 @@ void BMI088_Gyroscope::RunImpl()
 		// GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
 		// Following a delay of 30 ms, all configuration settings are overwritten with their reset value.
 		RegisterWrite(Register::GYRO_SOFTRESET, 0xB6);
-		DataReadyInterruptDisable();
 		_reset_timestamp = now;
 		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
@@ -113,7 +112,7 @@ void BMI088_Gyroscope::RunImpl()
 		if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) {
 			// if reset succeeded then configure
 			_state = STATE::CONFIGURE;
-			ScheduleDelayed(10_ms);
+			ScheduleDelayed(1_ms);

 		} else {
 			// RESET not complete
@@ -132,9 +131,21 @@ void BMI088_Gyroscope::RunImpl()

 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// if configure succeeded then reset the FIFO
-			_state = STATE::FIFO_RESET;
-			ScheduleDelayed(10_ms);
+			// if configure succeeded then start reading from FIFO
+			_state = STATE::FIFO_READ;
+
+			if (DataReadyInterruptConfigure()) {
+				_data_ready_interrupt_enabled = true;
+
+				// backup schedule as a watchdog timeout
+				ScheduleDelayed(100_ms);
+
+			} else {
+				_data_ready_interrupt_enabled = false;
+				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+			}
+
+			FIFOReset();

 		} else {
 			// CONFIGURE not complete
@@ -151,25 +162,6 @@ void BMI088_Gyroscope::RunImpl()

 		break;

-	case STATE::FIFO_RESET:
-		// if configure succeeded then start reading from FIFO
-		_state = STATE::FIFO_READ;
-
-		FIFOReset();
-
-		if (DataReadyInterruptConfigure()) {
-			_data_ready_interrupt_enabled = true;
-
-			// backup schedule as a watchdog timeout
-			ScheduleDelayed(100_ms);
-
-		} else {
-			_data_ready_interrupt_enabled = false;
-			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-		}
-
-		break;
-
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = 0;

@@ -33,8 +33,6 @@

 #include "ICM42688P.hpp"

-#define PRINT_REGISTER(REG) printf("%s (0x%02hhX) = 0x%02hhX \n", #REG, static_cast<uint8_t>(REG), RegisterRead(REG))
-
 using namespace time_literals;

 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
@@ -42,6 +40,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 	return (msb << 8u) | lsb;
 }

+static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
 ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
 	SPI(config),
 	I2CSPIDriver(config),
@@ -56,6 +59,7 @@ ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
 	if (config.custom1 != 0) {
 		_enable_clock_input = true;
 		_input_clock_freq = config.custom1;
+		ConfigureCLKIN();

 	} else {
 		_enable_clock_input = false;
@@ -66,13 +70,11 @@ ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :

 ICM42688P::~ICM42688P()
 {
-	perf_free(_reset_perf);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
 	perf_free(_fifo_empty_perf);
 	perf_free(_fifo_overflow_perf);
 	perf_free(_fifo_reset_perf);
-	perf_free(_fifo_timestamp_error_perf);
 	perf_free(_drdy_missed_perf);
 }

@@ -107,26 +109,20 @@ void ICM42688P::print_status()
 {
 	I2CSPIDriverBase::print_status();

-	PX4_INFO_RAW("FIFO empty interval: %d us (%.1f Hz)\n", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+	PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");

-	if (_enable_clock_input) {
-		PX4_INFO_RAW("Clock input: %.1f Hz\n", (double)_input_clock_freq);
-	}
-
-	perf_print_counter(_reset_perf);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_fifo_empty_perf);
 	perf_print_counter(_fifo_overflow_perf);
 	perf_print_counter(_fifo_reset_perf);
-	perf_print_counter(_fifo_timestamp_error_perf);
 	perf_print_counter(_drdy_missed_perf);
 }

 int ICM42688P::probe()
 {
-	// 3 retries
-	for (int retry = 0; retry < 3; retry++) {
+	for (int i = 0; i < 3; i++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);

 		if (whoami == WHOAMI) {
@@ -155,14 +151,12 @@ void ICM42688P::RunImpl()

 	switch (_state) {
 	case STATE::RESET:
-		perf_count(_reset_perf);
 		// DEVICE_CONFIG: Software reset configuration
 		RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
 		_reset_timestamp = now;
 		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
-
-		ScheduleDelayed(2_ms); // wait at least 1 ms for soft reset to be effective
+		ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
 		break;

 	case STATE::WAIT_FOR_RESET:
@@ -183,8 +177,8 @@ void ICM42688P::RunImpl()
 				ScheduleDelayed(100_ms);

 			} else {
-				PX4_DEBUG("Reset not complete, check again in 100 ms");
-				ScheduleDelayed(100_ms);
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
 			}
 		}

@@ -237,7 +231,7 @@ void ICM42688P::RunImpl()
 				// 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) {
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
 					timestamp_sample = drdy_timestamp_sample;
 					samples = _fifo_gyro_samples;

@@ -249,8 +243,6 @@ void ICM42688P::RunImpl()
 				ScheduleDelayed(_fifo_empty_interval_us * 2);
 			}

-			bool success = false;
-
 			if (samples == 0) {
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
@@ -263,42 +255,27 @@ void ICM42688P::RunImpl()
 					perf_count(_fifo_empty_perf);

 				} else {
-					// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
-					samples = fifo_count / sizeof(FIFO::DATA);
+					// FIFO count (size in bytes)
+					samples = (fifo_count / sizeof(FIFO::DATA));

-					if (samples > _fifo_gyro_samples) {
-						// grab desired number of samples, but reschedule next cycle sooner
-						const int extra_samples = samples - _fifo_gyro_samples;
-						samples = _fifo_gyro_samples;
-
-						if (_fifo_gyro_samples > extra_samples) {
-							// reschedule to run when a total of _fifo_gyro_samples should be available in the FIFO
-							const uint32_t reschedule_delay_us = (_fifo_gyro_samples - extra_samples) * static_cast<int>(FIFO_SAMPLE_DT);
-							ScheduleOnInterval(_fifo_empty_interval_us, reschedule_delay_us);
-
-						} else {
-							// otherwise reschedule to run immediately
-							ScheduleOnInterval(_fifo_empty_interval_us);
-						}
-
-					} else if (samples < _fifo_gyro_samples) {
-						// reschedule next cycle to catch the desired number of samples
-						ScheduleOnInterval(_fifo_empty_interval_us, (_fifo_gyro_samples - samples) * static_cast<int>(FIFO_SAMPLE_DT));
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
 					}

-					if (samples == _fifo_gyro_samples) {
-						if (FIFORead(now, samples)) {
-							success = true;
-
-							if (_failure_count > 0) {
-								_failure_count--;
-							}
-						}
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
 					}
 				}
 			}

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

@@ -313,14 +290,13 @@ void ICM42688P::RunImpl()

 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
-					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
 			}

-			// check configuration registers periodically or immediately following any failure
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
 				    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
 				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
@@ -348,10 +324,10 @@ void ICM42688P::ConfigureSampleRate(int sample_rate)
 	const float min_interval = FIFO_SAMPLE_DT;
 	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);

-	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES));
+	_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));

 	// recompute FIFO empty interval (us) with actual gyro sample limit
-	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / RATE);
+	_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);

 	ConfigureFIFOWatermark(_fifo_gyro_samples);
 }
@@ -362,21 +338,13 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
 	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);

 	for (auto &r : _register_bank0_cfg) {
-		switch (r.reg) {
-		case Register::BANK_0::FIFO_CONFIG2:
-			// FIFO_CONFIG2 7:0 FIFO_WM[7:0]
+		if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
+			// FIFO_WM[7:0]  FIFO_CONFIG2
 			r.set_bits = fifo_watermark_threshold & 0xFF;
-			r.clear_bits = ~r.set_bits;
-			break;

-		case Register::BANK_0::FIFO_CONFIG3:
-			// FIFO_CONFIG3 3:0 FIFO_WM[11:8]
+		} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
+			// FIFO_WM[11:8] FIFO_CONFIG3
 			r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
-			r.clear_bits = ~r.set_bits & 0x0F;
-			break;
-
-		default:
-			break;
 		}
 	}
 }
@@ -385,31 +353,14 @@ void ICM42688P::ConfigureCLKIN()
 {
 	for (auto &r0 : _register_bank0_cfg) {
 		if (r0.reg == Register::BANK_0::INTF_CONFIG1) {
-			if (_enable_clock_input) {
-				// RTC_MODE: 1: RTC clock input is required
-				r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
-				r0.clear_bits = 0;
-
-			} else {
-				// RTC_MODE: 0: No input RTC clock is required
-				r0.set_bits = 0;
-				r0.clear_bits = INTF_CONFIG1_BIT::RTC_MODE;
-			}
+			r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
 		}
 	}

 	for (auto &r1 : _register_bank1_cfg) {
 		if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
-			if (_enable_clock_input) {
-				// PIN9_FUNCTION CLKIN
-				r1.set_bits   = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
-				r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
-
-			} else {
-				// PIN9_FUNCTION reset/clear
-				r1.set_bits   = INTF_CONFIG5_BIT::PIN9_FUNCTION_RESET_SET;
-				r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_RESET_CLEAR;
-			}
+			r1.set_bits = PIN9_FUNCTION_BIT::CLKIN;
+			r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9;
 		}
 	}
 }
@@ -429,8 +380,6 @@ void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)

 bool ICM42688P::Configure()
 {
-	ConfigureCLKIN();
-
 	// first set and clear all configured register bits
 	for (const auto &reg_cfg : _register_bank0_cfg) {
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
@@ -444,7 +393,6 @@ bool ICM42688P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}

-
 	// now check that all are configured
 	bool success = true;

@@ -466,6 +414,11 @@ bool ICM42688P::Configure()
 		}
 	}

+	// 20-bits data format used
+	//  the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+	_px4_gyro.set_range(math::radians(2000.f));
+
 	return success;
 }

@@ -488,7 +441,7 @@ bool ICM42688P::DataReadyInterruptConfigure()
 	}

 	// Setup data ready on falling edge
-	return (px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0);
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
 }

 bool ICM42688P::DataReadyInterruptDisable()
@@ -497,7 +450,7 @@ bool ICM42688P::DataReadyInterruptDisable()
 		return false;
 	}

-	return (px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0);
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }

 template <typename T>
@@ -553,22 +506,117 @@ void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbit
 uint16_t ICM42688P::FIFOReadCount()
 {
 	// 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::BANK_SEL_0);

-	// transfer buffer
-	struct TransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-	} buffer{};
-
-	// read FIFO count
-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
+	if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
 		return 0;
 	}

-	return (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
+	return combine(fifo_count_buf[1], fifo_count_buf[2]);
+}
+
+bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
+	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
+		perf_count(_fifo_overflow_perf);
+		FIFOReset();
+		return false;
+	}
+
+	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
+
+	if (fifo_count_bytes >= FIFO::SIZE) {
+		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) {
+		perf_count(_fifo_empty_perf);
+		return false;
+	}
+
+	// check FIFO header in every sample
+	uint8_t valid_samples = 0;
+
+	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
+		bool valid = true;
+
+		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
+		const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
+
+		if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
+			// FIFO sample empty if HEADER_MSG set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
+			// accel bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
+			// gyro bit not set
+			valid = false;
+
+		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
+			// Packet does not contain ODR timestamp
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
+		}
+
+		if (valid) {
+			valid_samples++;
+
+		} else {
+			perf_count(_bad_transfer_perf);
+			break;
+		}
+	}
+
+	if (valid_samples > 0) {
+		if (ProcessTemperature(buffer.f, valid_samples)) {
+			ProcessGyro(timestamp_sample, buffer.f, valid_samples);
+			ProcessAccel(timestamp_sample, buffer.f, valid_samples);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void ICM42688P::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// SIGNAL_PATH_RESET: FIFO flush
+	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+
+	// reset while FIFO is disabled
+	_drdy_timestamp_sample.store(0);
 }

 static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
@@ -588,285 +636,224 @@ static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, co
 	return static_cast<int32_t>(x);
 }

-bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
 {
-	// FIFO transfer buffer
-	struct FIFOTransferBuffer {
-		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
-		uint8_t INT_STATUS{0};
-		uint8_t FIFO_COUNTH{0};
-		uint8_t FIFO_COUNTL{0};
-		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
-	} buffer{};
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;

-	// cmd + INT_STATUS + FIFO_COUNTH + FIFO_COUNTL + samples (FIFO::DATA)
-	const size_t transfer_size = 4 + math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE);
+	// 18-bits of accelerometer data
+	bool scale_20bit = false;

-	SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
+	// first pass
+	for (int i = 0; i < samples; i++) {

-	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
-		perf_count(_bad_transfer_perf);
-		return false;
+		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+		if (_enable_clock_input) {
+			accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+		// Accel data is 18 bit ()
+		int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
+						   fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
+		int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
+						   fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
+		int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
+						   fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
+
+		// sample invalid if -524288
+		if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
+			// check if any values are going to exceed int16 limits
+			static constexpr int16_t max_accel = INT16_MAX;
+			static constexpr int16_t min_accel = INT16_MIN;
+
+			if (accel_x >= max_accel || accel_x <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_y >= max_accel || accel_y <= min_accel) {
+				scale_20bit = true;
+			}
+
+			if (accel_z >= max_accel || accel_z <= min_accel) {
+				scale_20bit = true;
+			}
+
+			// shift by 2 (2 least significant bits are always 0)
+			accel.x[accel.samples] = accel_x / 4;
+			accel.y[accel.samples] = accel_y / 4;
+			accel.z[accel.samples] = accel_z / 4;
+			accel.samples++;
+		}
 	}

-	if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
+	if (!scale_20bit) {
+		// if highres enabled accel data is always 8192 LSB/g
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
+
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			// 20 bit hires mode
+			// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
+			// Accel data is 18 bit ()
+			int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
+			int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
+			int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
+
+			accel.x[i] = accel_x;
+			accel.y[i] = accel_y;
+			accel.z[i] = accel_z;
+		}
+
+		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
 	}

-	const uint16_t fifo_count_bytes = (buffer.FIFO_COUNTH << 8) + buffer.FIFO_COUNTL;
-
-	if (fifo_count_bytes >= FIFO::SIZE) {
-		perf_count(_fifo_overflow_perf);
-		FIFOReset();
-		return false;
+	// correct frame for publication
+	for (int i = 0; i < accel.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		accel.x[i] = accel.x[i];
+		accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
+		accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
 	}

-	const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
+	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));

-	if (fifo_count_samples == 0) {
-		perf_count(_fifo_empty_perf);
-		return false;
+	if (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+	}
+}
+
+void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
+{
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = 0;
+
+	// 20-bits of gyroscope data
+	bool scale_20bit = false;
+
+	// first pass
+	for (int i = 0; i < samples; i++) {
+
+		uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
+
+		if (_enable_clock_input) {
+			gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
+
+		} else {
+			gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
+		}
+
+		// 20 bit hires mode
+		// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
+		int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
+		int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
+		int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
+
+		// check if any values are going to exceed int16 limits
+		static constexpr int16_t max_gyro = INT16_MAX;
+		static constexpr int16_t min_gyro = INT16_MIN;
+
+		if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
+			scale_20bit = true;
+		}
+
+		gyro.x[gyro.samples] = gyro_x / 2;
+		gyro.y[gyro.samples] = gyro_y / 2;
+		gyro.z[gyro.samples] = gyro_z / 2;
+		gyro.samples++;
 	}

-	sensor_accel_fifo_s sensor_accel_fifo{};
-	sensor_accel_fifo.timestamp_sample = timestamp_sample;
-	sensor_accel_fifo.device_id = get_device_id();
-	sensor_accel_fifo.dt = FIFO_SAMPLE_DT;
-	sensor_accel_fifo.scale = CONSTANTS_ONE_G / 8192.f; // highres accel data 8192 LSB/g
+	if (!scale_20bit) {
+		// if highres enabled gyro data is always 131 LSB/dps
+		_px4_gyro.set_scale(math::radians(1.f / 131.f));

-	sensor_gyro_fifo_s sensor_gyro_fifo{};
-	sensor_gyro_fifo.timestamp_sample = timestamp_sample;
-	sensor_gyro_fifo.device_id = get_device_id();
-	sensor_gyro_fifo.dt = FIFO_SAMPLE_DT;
-	sensor_gyro_fifo.scale = math::radians(1.f / 131.f); // highres gyro data 131 LSB/dps
+	} else {
+		// 20 bit data scaled to 16 bit (2^4)
+		for (int i = 0; i < samples; i++) {
+			gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
+			gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
+			gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
+		}

-	// check FIFO header in every sample
-	uint8_t valid_samples = 0;
-	float temperature_sum = 0;
-	float timestamp_interval_sum = 0;
+		_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	}

-	bool accel_scale_16bit = false; // 18-bits of accelerometer data
-	bool gyro_scale_16bit = false; // 20-bits of gyroscope data
+	// correct frame for publication
+	for (int i = 0; i < gyro.samples; i++) {
+		// sensor's frame is +x forward, +y left, +z up
+		//  flip y & z to publish right handed with z down (x forward, y right, z down)
+		gyro.x[i] = gyro.x[i];
+		gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
+		gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
+	}

-	for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
-		const FIFO::DATA &fifo = buffer.f[i];
+	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
+				  perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));

-		// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
-		const uint8_t FIFO_HEADER = fifo.FIFO_Header;
+	if (gyro.samples > 0) {
+		_px4_gyro.updateFIFO(gyro);
+	}
+}

-		const bool HEADER_MSG       = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG; // FIFO is empty
-		const bool HEADER_ACCEL     = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL;
-		const bool HEADER_GYRO      = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO;
-		const bool HEADER_20        = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20; // valid sample of extended 20-bit data
-		// 3:2 HEADER_TIMESTAMP_FSYNC
-		const bool HEADER_ODR_ACCEL = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL; // ODR for accel is different
-		const bool HEADER_ODR_GYRO  = FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO;  // ODR for gyro is different
+bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
+{
+	int16_t temperature[FIFO_MAX_SAMPLES];
+	float temperature_sum{0};

-		if (!HEADER_MSG && HEADER_ACCEL && HEADER_GYRO && HEADER_20 && !HEADER_ODR_ACCEL && !HEADER_ODR_GYRO) {
+	int valid_samples = 0;

-			if (!accel_scale_16bit) {
-				// Accel: 20 bit hires mode, Accel data is 18 bit
-				// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
-				int32_t accel_x = reassemble_20bit(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0xF0 >> 4);
-				int32_t accel_y = reassemble_20bit(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
-				int32_t accel_z = reassemble_20bit(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
-
-				// sample invalid if -524288
-				if (accel_x == -524288 || accel_y == -524288 || accel_z == -524288) {
-					return false;
-				}
-
-				// shift by 2 (2 least significant bits are always 0)
-				accel_x = accel_x / 4;
-				accel_y = accel_y / 4;
-				accel_z = accel_z / 4;
-
-				// check if any values are going to exceed int16 limits
-				if ((accel_x >= INT16_MAX || accel_x <= INT16_MIN)
-				    || (accel_y >= INT16_MAX || accel_y <= INT16_MIN)
-				    || (accel_z >= INT16_MAX || accel_z <= INT16_MIN)) {
-
-					accel_scale_16bit = true;
-
-					// 20 bit data scaled to 16 bit
-					sensor_accel_fifo.scale = CONSTANTS_ONE_G / 2048.f;
-
-					// rescale any existing data
-					for (int j = 0; j < valid_samples + 1; j++) {
-						sensor_accel_fifo.x[j] = combine(buffer.f[j].ACCEL_DATA_X1, buffer.f[j].ACCEL_DATA_X0);
-						sensor_accel_fifo.y[j] = combine(buffer.f[j].ACCEL_DATA_Y1, buffer.f[j].ACCEL_DATA_Y0);
-						sensor_accel_fifo.z[j] = combine(buffer.f[j].ACCEL_DATA_Z1, buffer.f[j].ACCEL_DATA_Z0);
-					}
-
-				} else {
-					sensor_accel_fifo.x[valid_samples] = accel_x;
-					sensor_accel_fifo.y[valid_samples] = accel_y;
-					sensor_accel_fifo.z[valid_samples] = accel_z;
-				}
-
-			} else {
-				sensor_accel_fifo.x[valid_samples] = combine(fifo.ACCEL_DATA_X1, fifo.ACCEL_DATA_X0);
-				sensor_accel_fifo.y[valid_samples] = combine(fifo.ACCEL_DATA_Y1, fifo.ACCEL_DATA_Y0);
-				sensor_accel_fifo.z[valid_samples] = combine(fifo.ACCEL_DATA_Z1, fifo.ACCEL_DATA_Z0);
-			}
-
-
-			if (!gyro_scale_16bit) {
-				// Gyro: 20 bit hires mode
-				// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
-				int32_t gyro_x = reassemble_20bit(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0, fifo.Ext_Accel_X_Gyro_X & 0x0F);
-				int32_t gyro_y = reassemble_20bit(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0, fifo.Ext_Accel_Y_Gyro_Y & 0x0F);
-				int32_t gyro_z = reassemble_20bit(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0, fifo.Ext_Accel_Z_Gyro_Z & 0x0F);
-
-				// shift by 1 (least significant bit is always 0)
-				gyro_x = gyro_x / 2;
-				gyro_y = gyro_y / 2;
-				gyro_z = gyro_z / 2;
-
-				// check if any gyro values are going to exceed int16 limits
-				if ((gyro_x >= INT16_MAX || gyro_x <= INT16_MIN)
-				    || (gyro_y >= INT16_MAX || gyro_y <= INT16_MIN)
-				    || (gyro_z >= INT16_MAX || gyro_z <= INT16_MIN)) {
-
-					gyro_scale_16bit = true;
-
-					// 20 bit data scaled to 16 bit
-					sensor_gyro_fifo.scale = math::radians(2000.f / 32768.f);
-
-					// rescale any existing data
-					for (int j = 0; j < valid_samples + 1; j++) {
-						sensor_gyro_fifo.x[j] = combine(buffer.f[j].GYRO_DATA_X1, buffer.f[j].GYRO_DATA_X0);
-						sensor_gyro_fifo.y[j] = combine(buffer.f[j].GYRO_DATA_Y1, buffer.f[j].GYRO_DATA_Y0);
-						sensor_gyro_fifo.z[j] = combine(buffer.f[j].GYRO_DATA_Z1, buffer.f[j].GYRO_DATA_Z0);
-					}
-
-				} else {
-					sensor_gyro_fifo.x[valid_samples] = gyro_x;
-					sensor_gyro_fifo.y[valid_samples] = gyro_y;
-					sensor_gyro_fifo.z[valid_samples] = gyro_z;
-				}
-
-			} else {
-				sensor_gyro_fifo.x[i] = combine(fifo.GYRO_DATA_X1, fifo.GYRO_DATA_X0);
-				sensor_gyro_fifo.y[i] = combine(fifo.GYRO_DATA_Y1, fifo.GYRO_DATA_Y0);
-				sensor_gyro_fifo.z[i] = combine(fifo.GYRO_DATA_Z1, fifo.GYRO_DATA_Z0);
-			}
-
-
-			// temperature
-			const int16_t TEMP_DATA = combine(fifo.TEMP_DATA1, fifo.TEMP_DATA0);
-
-			// sample invalid if -32768
-			if (TEMP_DATA == -32768) {
-				return false;
-			}
-
-			temperature_sum += TEMP_DATA;
-
-
-			// HEADER_TIMESTAMP_FSYNC - 0b10: Packet contains ODR Timestamp
-			if (!(FIFO_HEADER & Bit3)) {
-				PX4_DEBUG("header timestamp missing");
-				return false;
-			}
-
-
-			const uint16_t time_delta = (fifo.TimeStamp_h << 8) + fifo.TimeStamp_l;
-
-			if (_enable_clock_input) {
-				// RTC_MODE=1, TMST_RES=0 timestamp interval reported in FIFO requires scaling by a factor of 32.768/RTC Frequency.
-				const float kFifoIntervalScale = static_cast<float>(32768) / _input_clock_freq;
-				timestamp_interval_sum += (float)time_delta * kFifoIntervalScale;
-
-			} else {
-				// RTC_MODE=0, TMST_RES=0 - timestamp interval reported in FIFO requires scaling by a factor of 32/30
-				static constexpr float kFifoIntervalScale = 32.f / 30.f;
-				timestamp_interval_sum += (float)time_delta * kFifoIntervalScale;
-			}
+	for (int i = 0; i < samples; i++) {
+		const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);

+		// sample invalid if -32768
+		if (t != -32768) {
+			temperature_sum += t;
+			temperature[valid_samples] = t;
 			valid_samples++;
 		}
 	}

 	if (valid_samples > 0) {
-
-		sensor_accel_fifo.samples = valid_samples;
-		sensor_gyro_fifo.samples = valid_samples;
+		const float temperature_avg = temperature_sum / valid_samples;

 		for (int i = 0; i < valid_samples; i++) {
-			// sensor's frame is +x forward, +y left, +z up
-			//  flip y & z to publish right handed with z down (x forward, y right, z down)
-
-			// sensor_accel_fifo.x[i] no change
-			sensor_accel_fifo.y[i] = math::negate(sensor_accel_fifo.y[i]);
-			sensor_accel_fifo.z[i] = math::negate(sensor_accel_fifo.z[i]);
-			//rotate_3i(_rotation, sensor_accel_fifo.x[i], sensor_accel_fifo.y[i], sensor_accel_fifo.z[i]);
-
-			// sensor_gyro_fifo.x[i] no change
-			sensor_gyro_fifo.y[i] = math::negate(sensor_gyro_fifo.y[i]);
-			sensor_gyro_fifo.z[i] = math::negate(sensor_gyro_fifo.z[i]);
-			//rotate_3i(_rotation, sensor_gyro_fifo.x[i], sensor_gyro_fifo.y[i], sensor_gyro_fifo.z[i]);
+			// temperature changing wildly is an indication of a transfer error
+			if (fabsf(temperature[i] - temperature_avg) > 1000) {
+				perf_count(_bad_transfer_perf);
+				return false;
+			}
 		}

-		const float temperature_avg = temperature_sum / valid_samples;
-
 		// use average temperature reading
 		const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;

 		if (PX4_ISFINITE(TEMP_degC)) {
 			_px4_accel.set_temperature(TEMP_degC);
 			_px4_gyro.set_temperature(TEMP_degC);
+			return true;

 		} else {
 			perf_count(_bad_transfer_perf);
-			return false;
 		}
-
-		if (timestamp_interval_sum > 0) {
-			const float dt_avg = (timestamp_interval_sum / valid_samples);
-
-			// check dt is within +=1% of expected value
-			if ((dt_avg < (FIFO_SAMPLE_DT * 0.99f)) || (dt_avg > (FIFO_SAMPLE_DT * 1.01f))) {
-				perf_count(_fifo_timestamp_error_perf);
-
-			} else {
-				sensor_accel_fifo.dt = dt_avg;
-				sensor_gyro_fifo.dt = dt_avg;
-			}
-		}
-
-		const uint32_t error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
-					     perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf);
-
-		_px4_accel.set_error_count(error_count);
-		_px4_gyro.set_error_count(error_count);
-
-		_px4_accel.set_scale(sensor_accel_fifo.scale);
-		_px4_gyro.set_scale(sensor_gyro_fifo.scale);
-
-		// publish accel
-		sensor_accel_fifo.timestamp = hrt_absolute_time();
-		_px4_accel.updateFIFO(sensor_accel_fifo);
-
-		// publish gyro
-		sensor_accel_fifo.timestamp = hrt_absolute_time();
-		_px4_gyro.updateFIFO(sensor_gyro_fifo);
-
-		return true;
 	}

 	return false;
 }
-
-void ICM42688P::FIFOReset()
-{
-	perf_count(_fifo_reset_perf);
-
-	// SIGNAL_PATH_RESET: FIFO flush
-	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
-
-	// reset while FIFO is disabled
-	_drdy_timestamp_sample.store(0);
-}
@@ -71,10 +71,24 @@ private:

 	// Sensor Configuration
 	static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};     // 8000 Hz accel & gyro ODR configured
-	static constexpr float RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
+	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
+
+	static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input

 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	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]), 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_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 {
+		uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
+		uint8_t INT_STATUS{0};
+		uint8_t FIFO_COUNTH{0};
+		uint8_t FIFO_COUNTL{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));

 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -124,18 +138,20 @@ private:
 	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
 	void FIFOReset();

+	void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
+	bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
+
 	const spi_drdy_gpio_t _drdy_gpio;

 	PX4Accelerometer _px4_accel;
 	PX4Gyroscope _px4_gyro;

-	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
 	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 _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
 	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
 	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
-	perf_counter_t _fifo_timestamp_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO timestamp error")};
 	perf_counter_t _drdy_missed_perf{nullptr};

 	hrt_abstime _reset_timestamp{0};
@@ -160,7 +176,7 @@ private:
 	} _state{STATE::RESET};

 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / 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{16};
@@ -168,39 +184,34 @@ private:
 		// Register                              | Set bits, Clear bits
 		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
 		{ Register::BANK_0::FIFO_CONFIG,          FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
-		{ Register::BANK_0::INTF_CONFIG1,         0, INTF_CONFIG1_BIT::RTC_MODE}, // RTC_MODE set at runtime if enabled
-		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, PWR_MGMT0_BIT::TEMP_DIS },
-		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G_CLEAR | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
-		{ Register::BANK_0::GYRO_CONFIG1,         GYRO_CONFIG1_BIT::TEMP_FILT_BW_5HZ, GYRO_CONFIG1_BIT::GYRO_UI_FILT_1ST_ORDER_CLEAR },
-		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW_MAX_CLEAR | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW_MAX_CLEAR },
-		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_1ST_ORDER_CLEAR },
-		{ Register::BANK_0::TMST_CONFIG,          TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_EN, TMST_CONFIG_BIT::TMST_RES | TMST_CONFIG_BIT::TMST_FSYNC_EN },
-		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH },
-		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0]  set at runtime
+		{ Register::BANK_0::INTF_CONFIG1,         0, 0}, // RTC_MODE[2] set at runtime
+		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
+		{ Register::BANK_0::GYRO_ACCEL_CONFIG0,   0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
+		{ Register::BANK_0::TMST_CONFIG,          TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
 		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
-		{ Register::BANK_0::INT_CONFIG1,          INT_CONFIG1_BIT::INT_TPULSE_DURATION | INT_CONFIG1_BIT::INT_TDEASSERT_DISABLE, INT_CONFIG1_BIT::INT_ASYNC_RESET },
-		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, INT_SOURCE0_BIT::RESET_DONE_INT1_EN | INT_SOURCE0_BIT::UI_DRDY_INT1_EN | INT_SOURCE0_BIT::FIFO_FULL_INT1_EN },
+		{ Register::BANK_0::INT_CONFIG0,          INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
+		{ Register::BANK_0::INT_CONFIG1,          0, INT_CONFIG1_BIT::INT_ASYNC_RESET },
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
 	};

 	uint8_t _checked_register_bank1{0};
-	static constexpr uint8_t size_register_bank1_cfg{5};
+	static constexpr uint8_t size_register_bank1_cfg{2};
 	register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
 		// Register                              | Set bits, Clear bits
-		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
-		{ Register::BANK_1::GYRO_CONFIG_STATIC3,  GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC4,  GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR},
-		{ Register::BANK_1::GYRO_CONFIG_STATIC5,  GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR},
+		{ Register::BANK_1::GYRO_CONFIG_STATIC2,  GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS, 0 },
 		{ Register::BANK_1::INTF_CONFIG5,         0, 0 },
 	};

 	uint8_t _checked_register_bank2{0};
-	static constexpr uint8_t size_register_bank2_cfg{3};
+	static constexpr uint8_t size_register_bank2_cfg{1};
 	register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
 		// Register                              | Set bits, Clear bits
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR },
-		{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR },
+		{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS, 0 },
 	};
 };
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020 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
@@ -82,7 +82,7 @@ enum class BANK_0 : uint8_t {
 	FIFO_DATA          = 0x30,

 	SIGNAL_PATH_RESET  = 0x4B,
-
+	INTF_CONFIG0       = 0x4C,
 	INTF_CONFIG1       = 0x4D,
 	PWR_MGMT0          = 0x4E,
 	GYRO_CONFIG0       = 0x4F,
@@ -108,18 +108,12 @@ enum class BANK_0 : uint8_t {
 };

 enum class BANK_1 : uint8_t {
-	GYRO_CONFIG_STATIC2  = 0x0B,
-	GYRO_CONFIG_STATIC3  = 0x0C,
-	GYRO_CONFIG_STATIC4  = 0x0D,
-	GYRO_CONFIG_STATIC5  = 0x0E,
-
-	INTF_CONFIG5         = 0x7B,
+	GYRO_CONFIG_STATIC2 = 0x0B,
+	INTF_CONFIG5        = 0x7B,
 };

 enum class BANK_2 : uint8_t {
 	ACCEL_CONFIG_STATIC2 = 0x03,
-	ACCEL_CONFIG_STATIC3 = 0x04,
-	ACCEL_CONFIG_STATIC4 = 0x05,
 };

 };
@@ -128,14 +122,14 @@ enum class BANK_2 : uint8_t {

 // DEVICE_CONFIG
 enum DEVICE_CONFIG_BIT : uint8_t {
-	SOFT_RESET_CONFIG = Bit0,
+	SOFT_RESET_CONFIG = Bit0, //
 };

 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2, // 1: Latched mode
-	INT1_DRIVE_CIRCUIT  = Bit1, // 1: Push pull
-	INT1_POLARITY       = Bit0, // 1: Active high
+	INT1_MODE           = Bit2,
+	INT1_DRIVE_CIRCUIT  = Bit1,
+	INT1_POLARITY       = Bit0,
 };

 // FIFO_CONFIG
@@ -155,101 +149,103 @@ enum INT_STATUS_BIT : uint8_t {
 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
 	ABORT_AND_RESET = Bit3,
-
 	FIFO_FLUSH      = Bit1,
 };

 enum INTF_CONFIG1_BIT : uint8_t {
-	RTC_MODE     = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
+	RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
+	CLKSEL = Bit0,
+	CLKSEL_CLEAR = Bit1,
 };

 // PWR_MGMT0
 enum PWR_MGMT0_BIT : uint8_t {
-	TEMP_DIS             = Bit5, // 0: Temperature sensor is enabled (default)
-
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 0b11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 0b11: Places accelerometer in Low Noise (LN) Mode
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
 };

 // GYRO_CONFIG0
 enum GYRO_CONFIG0_BIT : uint8_t {
 	// 7:5 GYRO_FS_SEL
-	//  0b000: ±2000dps (default)
-	GYRO_FS_SEL_2000_DPS_CLEAR = Bit7 | Bit6 | Bit5,
+	GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
+	GYRO_FS_SEL_1000_DPS = Bit5,
+	GYRO_FS_SEL_500_DPS  = Bit6,
+	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,
+	GYRO_FS_SEL_125_DPS  = Bit7,
+

 	// 3:0 GYRO_ODR
-	//  0b0001: 32kHz (maximum)
-	GYRO_ODR_32KHZ_SET         = Bit0,
-	GYRO_ODR_32KHZ_CLEAR       = Bit3 | Bit2 | Bit0,
-	//  0b0010: 16kHz
-	GYRO_ODR_16KHZ_SET         = Bit1,
-	GYRO_ODR_16KHZ_CLEAR       = Bit3 | Bit2 | Bit0,
-	//  0b0011: 8kHz
-	GYRO_ODR_8KHZ_SET          = Bit1 | Bit0,
-	GYRO_ODR_8KHZ_CLEAR        = Bit3 | Bit2,
-	//  0b0110: 1kHz (default)
-	GYRO_ODR_1KHZ_SET          = Bit2 | Bit1,
-	GYRO_ODR_1KHZ_CLEAR        = Bit3 | Bit0,
+	//  0001: 32kHz
+	GYRO_ODR_32KHZ_SET   = Bit0,
+	GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	GYRO_ODR_16KHZ_SET   = Bit1,
+	GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	GYRO_ODR_8KHZ_SET    = Bit1 | Bit0,
+	GYRO_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	GYRO_ODR_1KHZ_SET    = Bit2 | Bit1,
+	GYRO_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
 };

 // ACCEL_CONFIG0
 enum ACCEL_CONFIG0_BIT : uint8_t {
 	// 7:5 ACCEL_FS_SEL
-	//  0b000: ±16g (default)
-	ACCEL_FS_SEL_16G_CLEAR     = Bit7 | Bit6 | Bit5,
+	ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
+	ACCEL_FS_SEL_8G  = Bit5,
+	ACCEL_FS_SEL_4G  = Bit6,
+	ACCEL_FS_SEL_2G  = Bit6 | Bit5,
+

 	// 3:0 ACCEL_ODR
-	//  0b0001: 32kHz (maximum)
-	ACCEL_ODR_32KHZ_SET        = Bit0,
-	ACCEL_ODR_32KHZ_CLEAR      = Bit3 | Bit2 | Bit0,
-	//  0b0010: 16kHz
-	ACCEL_ODR_16KHZ_SET        = Bit1,
-	ACCEL_ODR_16KHZ_CLEAR      = Bit3 | Bit2 | Bit0,
-	//  0b0011: 8kHz
-	ACCEL_ODR_8KHZ_SET         = Bit1 | Bit0,
-	ACCEL_ODR_8KHZ_CLEAR       = Bit3 | Bit2,
-	//  0b0110: 1kHz (default)
-	ACCEL_ODR_1KHZ_SET         = Bit2 | Bit1,
-	ACCEL_ODR_1KHZ_CLEAR       = Bit3 | Bit0,
+	//  0001: 32kHz
+	ACCEL_ODR_32KHZ_SET   = Bit0,
+	ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0010: 16kHz
+	ACCEL_ODR_16KHZ_SET   = Bit1,
+	ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
+	//  0011: 8kHz
+	ACCEL_ODR_8KHZ_SET    = Bit1 | Bit0,
+	ACCEL_ODR_8KHZ_CLEAR  = Bit3 | Bit2,
+	//  0110: 1kHz (default)
+	ACCEL_ODR_1KHZ_SET    = Bit2 | Bit1,
+	ACCEL_ODR_1KHZ_CLEAR  = Bit3 | Bit0,
 };

 // GYRO_CONFIG1
 enum GYRO_CONFIG1_BIT : uint8_t {
-	// 7:5 TEMP_FILT_BW
-	TEMP_FILT_BW_5HZ = Bit7 | Bit6 | Bit5, // 0b111: DLPF BW = 5Hz; DLPF Latency = 32ms
-
-	// 3:2 GYRO_UI_FILT_ORD
-	GYRO_UI_FILT_1ST_ORDER_CLEAR = Bit3 | Bit2, // 0b00: 1st Order
+	GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
 };

 // GYRO_ACCEL_CONFIG0
 enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
 	// 7:4 ACCEL_UI_FILT_BW
-	ACCEL_UI_FILT_BW_MAX_CLEAR = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
+	ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2

 	// 3:0 GYRO_UI_FILT_BW
-	GYRO_UI_FILT_BW_MAX_CLEAR  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
+	GYRO_UI_FILT_BW  = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
 };

 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-	// 4:3 ACCEL_UI_FILT_ORD
-	ACCEL_UI_FILT_1ST_ORDER_CLEAR = Bit4 | Bit3, // 00: 1st Order
+	ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
 };

 // TMST_CONFIG
 enum TMST_CONFIG_BIT : uint8_t {
-	TMST_RES      = Bit3, // Time Stamp resolution: When set to 0 (default), time stamp resolution is 1 µs.
-	TMST_DELTA_EN = Bit2, // Time Stamp delta enable
-	TMST_FSYNC_EN = Bit1, // Time Stamp register FSYNC enable
-	TMST_EN       = Bit0, // Time Stamp register enable
+	TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value
+	TMST_RES	= Bit3, // 0: 1us resolution, 1: 16us resolution or 1 RTC period when clock is used
+	TMST_DELTA_EN   = Bit2, // 1: Time Stamp delta enable
+	TMST_FSYNC_EN   = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled
+	TMST_EN		= Bit0, // 1: Time Stamp register enable (default)
 };

 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
 	FIFO_RESUME_PARTIAL_RD = Bit6,
-	FIFO_WM_GT_TH          = Bit5, // Trigger FIFO watermark interrupt on every ODR (DMA write) if FIFO_COUNT ≥ FIFO_WM_TH
-	FIFO_HIRES_EN          = Bit4, // Enable 3 bytes of extended 20-bits accel, gyro data
+	FIFO_WM_GT_TH          = Bit5,
+	FIFO_HIRES_EN          = Bit4,
 	FIFO_TMST_FSYNC_EN     = Bit3,
 	FIFO_TEMP_EN           = Bit2,
 	FIFO_GYRO_EN           = Bit1,
@@ -259,14 +255,12 @@ enum FIFO_CONFIG1_BIT : uint8_t {
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	FIFO_THS_INT_CLEAR = Bit3, // 0b10: Clear on FIFO data 1Byte Read
+	CLEAR_ON_FIFO_READ = Bit3,
 };

 // INT_CONFIG1
 enum INT_CONFIG1_BIT : uint8_t {
-	INT_TPULSE_DURATION   = Bit6, // 1: Interrupt pulse duration is 8 µs. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
-	INT_TDEASSERT_DISABLE = Bit5, // 1: Disables de-assert duration. Required if ODR ≥ 4kHz, optional for ODR < 4kHz.
-	INT_ASYNC_RESET       = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
+	INT_ASYNC_RESET = Bit4, // User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation
 };

 // INT_SOURCE0
@@ -295,85 +289,23 @@ enum REG_BANK_SEL_BIT : uint8_t {

 // GYRO_CONFIG_STATIC2
 enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
-	GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter
-	GYRO_NF_DIS  = Bit0, // 1: Disable Notch Filter
+	GYRO_AAF_DIS = Bit1,
+	GYRO_NF_DIS  = Bit0,
 };

-// GYRO_CONFIG_STATIC3
-enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
-	// 5:0 GYRO_AAF_DELT
-	//  585 Hz = 13 (0b00'1101)
-	GYRO_AAF_DELT_585HZ_SET   = Bit3 | Bit2 | Bit0,
-	GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1,
+// PIN9_FUNCTION
+enum PIN9_FUNCTION_BIT : uint8_t {
+	FSYNC_PIN9 = Bit1,
+	CLKIN = Bit2,
 };

-// GYRO_CONFIG_STATIC4
-enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
-	// 7:0 GYRO_AAF_DELTSQR
-	//  585 Hz = 170 (0b1010'1010)
-	GYRO_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
-	GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
-};
-
-// GYRO_CONFIG_STATIC5
-enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
-	// 7:4 GYRO_AAF_BITSHIFT
-	//  585 Hz = 8 (0b1000)
-	GYRO_AAF_BITSHIFT_585HZ_SET      = Bit7,
-	GYRO_AAF_BITSHIFT_585HZ_CLEAR    = Bit6 | Bit5 | Bit4,
-
-	// 3:0 GYRO_AAF_DELTSQR[11:8]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	GYRO_AAF_DELTSQR_MSB_585HZ_SET   = 0,
-	GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
-};
-
-// INTF_CONFIG5
-enum INTF_CONFIG5_BIT : uint8_t {
-	// 2:1 PIN9_FUNCTION
-	PIN9_FUNCTION_CLKIN_SET   = Bit2, // 0b10: CLKIN
-	PIN9_FUNCTION_CLKIN_CLEAR = Bit1,
-
-	PIN9_FUNCTION_RESET_SET   = 0,
-	PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1,
-};
-
-
 //---------------- BANK2 Register bits

 // ACCEL_CONFIG_STATIC2
 enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
-	// 6:1 ACCEL_AAF_DELT
-	//  585 Hz = 13 (0b00'1101)
-	ACCEL_AAF_DELT_585HZ_SET   = Bit4 | Bit3 | Bit1,
-	ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2,
-
-	// 0 ACCEL_AAF_DIS
-	ACCEL_AAF_DIS              = Bit0, // 0: Enable accelerometer anti-aliasing filter (default)
+	ACCEL_AAF_DIS = Bit0,
 };

-// ACCEL_CONFIG_STATIC3
-enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
-	// 7:0 ACCEL_AAF_DELTSQR[7:0]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	ACCEL_AAF_DELTSQR_LSB_585HZ_SET   = Bit7 | Bit5 | Bit3 | Bit1,
-	ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
-};
-
-// ACCEL_CONFIG_STATIC4
-enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
-	// 7:4 ACCEL_AAF_BITSHIFT
-	//  585 Hz = 8 (0b1000)
-	ACCEL_AAF_BITSHIFT_585HZ_SET   = Bit7,
-	ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
-
-	// 3:0 ACCEL_AAF_DELTSQR[11:8]
-	//  585 Hz = 170 (0b0000'1010'1010)
-	ACCEL_AAF_DELTSQR_MSB_SET      = 0,
-	ACCEL_AAF_DELTSQR_MSB_CLEAR    = Bit3 | Bit2 | Bit1 | Bit0,
-};
-
-
 namespace FIFO
 {
 static constexpr size_t SIZE = 2048;
@@ -406,13 +338,13 @@ struct DATA {

 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
 enum FIFO_HEADER_BIT : uint8_t {
-	HEADER_MSG             = Bit7,        // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6,        // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
-	HEADER_GYRO            = Bit5,        // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
-	HEADER_20              = Bit4,        // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
+	HEADER_MSG             = Bit7, // 1: FIFO is empty
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
-	HEADER_ODR_ACCEL       = Bit1,        // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
-	HEADER_ODR_GYRO        = Bit0,        // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
 };

 }
@@ -359,8 +359,8 @@ void IIM42652::ConfigureCLKIN()

 	for (auto &r1 : _register_bank1_cfg) {
 		if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
-			r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
-			r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
+			r1.set_bits = PIN9_FUNCTION_BIT::CLKIN;
+			r1.clear_bits = PIN9_FUNCTION_BIT::FSYNC_PIN9;
 		}
 	}
 }
--- a/Show More
+++ b/Show More