WIP: mavlink zenoh pico playground

rtl: reset rtl state only on activation
rtl state was getting reset on inactive, which meant that the state which triggered resuming e.g. mission landing would be overwritten, and the navigator mode would switch back and forth between rtl and mission. this commit: 1. moves the reset of rtl state to the on activation function (removing it from the on inactive function) 2. functionalizes the rtl state input to the rtl time estimator so that rtl time can still be calculated from state=none while inactive
2026-08-21 14:20:36 +08:00 · 2022-07-22 10:51:35 -04:00 · 2022-07-22 14:59:20 +02:00 · 2022-07-22 14:59:20 +02:00 · 2022-07-22 08:31:32 +02:00 · 2022-07-21 11:27:09 -04:00
215 changed files with 7539 additions and 3795 deletions
@@ -1,11 +1,17 @@
 ---
 Checks:          '*,
+                  -*-avoid-c-arrays,
+                  -*-uppercase-literal-suffix,
+                  -*-magic-numbers,
+                  -altera-id-dependent-backward-branch,
+                  -altera-unroll-loops,
                  -android*,
                  -bugprone-integer-division,
                  -cert-dcl50-cpp,
                  -cert-env33-c,
                  -cert-err34-c,
                  -cert-err58-cpp,
+                  -cert-flp30-c,
                  -cert-msc30-c,
                  -cert-msc50-cpp,
                  -clang-analyzer-core.CallAndMessage,
@@ -18,6 +24,7 @@ Checks:          '*,
                  -clang-analyzer-deadcode.DeadStores,
                  -clang-analyzer-optin.cplusplus.VirtualCall,
                  -clang-analyzer-optin.performance.Padding,
+                  -clang-analyzer-security.FloatLoopCounter,
                  -clang-analyzer-security.insecureAPI.strcpy,
                  -clang-analyzer-unix.API,
                  -clang-analyzer-unix.cstring.BadSizeArg,
@@ -37,8 +44,7 @@ Checks:          '*,
                  -cppcoreguidelines-pro-type-union-access,
                  -cppcoreguidelines-pro-type-vararg,
                  -cppcoreguidelines-special-member-functions,
-                  -fuchsia-default-arguments,
-                  -fuchsia-overloaded-operator,
+                  -fuchsia-*,
                  -google-build-using-namespace,
                  -google-explicit-constructor,
                  -google-global-names-in-headers,
@@ -62,6 +68,7 @@ Checks:          '*,
                  -hicpp-use-equals-delete,
                  -hicpp-use-override,
                  -hicpp-vararg,
+                  -llvmlibc-*,
                  -llvm-header-guard,
                  -llvm-include-order,
                  -llvm-namespace-comment,
@@ -84,6 +91,7 @@ Checks:          '*,
                  -modernize-use-override,
                  -modernize-use-trailing-return-type,
                  -modernize-use-using,
+                  -modernize-use-trailing-return-type,
                  -performance-inefficient-string-concatenation,
                  -readability-avoid-const-params-in-decls,
                  -readability-container-size-empty,
@@ -67,3 +67,6 @@
 [submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"]
 	path = src/modules/microdds_client/Micro-XRCE-DDS-Client
 	url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git
+[submodule "src/modules/mavlink/zenoh-pico"]
+	path = src/modules/mavlink/zenoh-pico
+	url = git@github.com:eclipse-zenoh/zenoh-pico.git
@@ -175,6 +175,12 @@ include(kconfig)
 message(STATUS "PX4 config: ${PX4_CONFIG}")
 message(STATUS "PX4 platform: ${PX4_PLATFORM}")

+if($ENV{CLION_IDE})
+	# CLion automatically executes some compiler commands after configuring the
+	# project. This would fail on NuttX, as visibility.h tries to (indirectly)
+	# include nuttx/config.h, which at that point does not exist yet
+	add_definitions(-DPX4_DISABLE_GCC_POISON)
+endif()

 if(${PX4_PLATFORM} STREQUAL "posix")
 	if(ENABLE_LOCKSTEP_SCHEDULER)
@@ -6,38 +6,38 @@

 [![Slack](/.github/slack.svg)](https://join.slack.com/t/px4/shared_invite/zt-si4xo5qs-R4baYFmMjlrT4rQK5yUnaA)

-This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
+This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/PX4-Autopilot/tree/main/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.

 PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out of the box.

 * Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/PX4-Autopilot/blob/master/LICENSE))
-* [Supported airframes](https://docs.px4.io/master/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
-  * [Multicopters](https://docs.px4.io/master/en/frames_multicopter/)
-  * [Fixed wing](https://docs.px4.io/master/en/frames_plane/)
-  * [VTOL](https://docs.px4.io/master/en/frames_vtol/)
-  * [Autogyro](https://docs.px4.io/master/en/frames_autogyro/)
-  * [Rover](https://docs.px4.io/master/en/frames_rover/)
+* [Supported airframes](https://docs.px4.io/main/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
+  * [Multicopters](https://docs.px4.io/main/en/frames_multicopter/)
+  * [Fixed wing](https://docs.px4.io/main/en/frames_plane/)
+  * [VTOL](https://docs.px4.io/main/en/frames_vtol/)
+  * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/)
+  * [Rover](https://docs.px4.io/main/en/frames_rover/)
  * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc)
 * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases)


 ## Building a PX4 based drone, rover, boat or robot

-The [PX4 User Guide](https://docs.px4.io/master/en/) explains how to assemble [supported vehicles](https://docs.px4.io/master/en/airframes/airframe_reference.html) and fly drones with PX4.
-See the [forum and chat](https://docs.px4.io/master/en/#support) if you need help!
+The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4.
+See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help!


 ## Changing code and contributing

-This [Developer Guide](https://docs.px4.io/master/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.
+This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.

-Developers should read the [Guide for Contributions](https://docs.px4.io/master/en/contribute/).
-See the [forum and chat](https://dev.px4.io/master/en/#support) if you need help!
+Developers should read the [Guide for Contributions](https://docs.px4.io/main/en/contribute/).
+See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help!


 ### Weekly Dev Call

-The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/master/en/contribute/#dev_call).
+The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/).

 > **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers. All regular calls are listed in the [Dronecode calendar](https://www.dronecode.org/calendar/).

@@ -88,35 +88,43 @@ This repository contains code supporting Pixhawk standard boards (best supported
  * FMUv6X and FMUv6U (STM32H7, 2021)
    * Various vendors will provide FMUv6X and FMUv6U based designs Q3/2021
  * FMUv5 and FMUv5X (STM32F7, 2019/20)
-    * [Pixhawk 4 (FMUv5)](https://docs.px4.io/master/en/flight_controller/pixhawk4.html)
-    * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/master/en/flight_controller/pixhawk4_mini.html)
-    * [CUAV V5+ (FMUv5)](https://docs.px4.io/master/en/flight_controller/cuav_v5_plus.html)
-    * [CUAV V5 nano (FMUv5)](https://docs.px4.io/master/en/flight_controller/cuav_v5_nano.html)
-    * [Auterion Skynode (FMUv5X)](https://docs.px4.io/master/en/flight_controller/auterion_skynode.html)
+    * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
+    * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
+    * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
+    * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
+    * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/skynode)
  * FMUv4 (STM32F4, 2015)
-    * [Pixracer](https://docs.px4.io/master/en/flight_controller/pixracer.html)
-    * [Pixhawk 3 Pro](https://docs.px4.io/master/en/flight_controller/pixhawk3_pro.html)
+    * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
+    * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
  * FMUv3 (STM32F4, 2014)
-    * [Pixhawk 2](https://docs.px4.io/master/en/flight_controller/pixhawk-2.html)
-    * [Pixhawk Mini](https://docs.px4.io/master/en/flight_controller/pixhawk_mini.html)
-    * [CUAV Pixhack v3](https://docs.px4.io/master/en/flight_controller/pixhack_v3.html)
+    * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
+    * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
+    * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
  * FMUv2 (STM32F4, 2013)
-    * [Pixhawk](https://docs.px4.io/master/en/flight_controller/pixhawk.html)
-    * [Pixfalcon](https://docs.px4.io/master/en/flight_controller/pixfalcon.html)
+    * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
+    * [Pixfalcon](https://docs.px4.io/main/en/flight_controller/pixfalcon.html)

 ### Manufacturer and Community supported
-  * [Holybro Durandal](https://docs.px4.io/master/en/flight_controller/durandal.html)
-  * [Hex Cube Orange](https://docs.px4.io/master/en/flight_controller/cubepilot_cube_orange.html)
-  * [Hex Cube Yellow](https://docs.px4.io/master/en/flight_controller/cubepilot_cube_yellow.html)
+  * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
+  * [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+  * [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
  * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
  * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/master/en/complete_vehicles/crazyflie2.html)
-  * [Omnibus F4 SD](https://docs.px4.io/master/en/flight_controller/omnibus_f4_sd.html)
-  * [Holybro Kakute F7](https://docs.px4.io/master/en/flight_controller/kakutef7.html)
-  * [Raspberry PI with Navio 2](https://docs.px4.io/master/en/flight_controller/raspberry_pi_navio2.html)
+  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)
+  * [Omnibus F4 SD](https://docs.px4.io/main/en/flight_controller/omnibus_f4_sd.html)
+  * [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
+  * [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)

-Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/master/en/flight_controller/).
+Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).

 ## Project Roadmap

 A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25).
+
+## Project Governance
+
+The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation.
+
+<a href="https://www.dronecode.org/" style="padding:20px" ><img src="https://mavlink.io/assets/site/logo_dronecode.png" alt="Dronecode Logo" width="110px"/></a>
+<a href="https://www.linuxfoundation.org/projects" style="padding:20px;"><img src="https://mavlink.io/assets/site/logo_linux_foundation.png" alt="Linux Foundation Logo" width="80px" /></a>
+<div style="padding:10px">&nbsp;</div>
@@ -10,7 +10,6 @@ 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 FW_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 12

@@ -29,7 +28,7 @@ param set-default FW_SPOILERS_LND 0.4

 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25

 param set-default FW_T_CLMB_MAX 8
 param set-default FW_T_SINK_MAX 2.7
@@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 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 FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1

 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10

 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
@@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 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 FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1

 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10

 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
@@ -10,7 +10,6 @@ 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 FW_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 15

@@ -7,12 +7,9 @@

 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 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 FW_LND_TLALT 15

 param set-default FW_L1_PERIOD 25

@@ -37,7 +34,6 @@ param set-default NAV_DLL_ACT 2

 param set-default RWTO_TKOFF 1
 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10
 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8

@@ -10,7 +10,6 @@ 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 FW_THR_LND_MAX 0

 param set-default FW_L1_PERIOD 12

@@ -27,7 +26,7 @@ param set-default FW_RR_P 0.3

 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25

 param set-default FW_T_ALT_TC 2
 param set-default FW_T_CLMB_MAX 8
@@ -5,5 +5,5 @@

 . ${R}etc/init.d-posix/airframes/1030_plane

-param set-default FW_THR_CRUISE 0.0
+param set-default FW_THR_TRIM 0.0
 param set-default RWTO_TKOFF 0
@@ -57,7 +57,7 @@ param set-default FW_P_LIM_MAX 32
 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_CRUISE 0.25
+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
@@ -51,7 +51,7 @@ param set-default FW_PSP_OFF 2
 param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_P 0.2
-param set-default FW_THR_CRUISE 0.33
+param set-default FW_THR_TRIM 0.33
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_ALT_TC 2
@@ -64,7 +64,7 @@ param set-default FW_P_LIM_MAX 32
 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_CRUISE 0.38
+param set-default FW_THR_TRIM 0.38
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_CLMB_MAX 8
@@ -16,7 +16,7 @@ param set-default FW_P_LIM_MAX 32
 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_CRUISE 0.25
+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_ALT_TC 2
@@ -41,7 +41,7 @@ param set-default FW_R_LIM 30
 param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0

-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8
 param set-default FW_THR_IDLE 0
 param set-default COM_DISARM_PRFLT 0

@@ -0,0 +1,55 @@
+#!/bin/sh
+#
+# @name ThunderFly TF-G2
+# ThunderFly TF-G2 autogyro airframe. Only for FlightGear simulator
+#
+# @type Autogyro
+# @class Autogyro
+#
+# @url https://github.com/ThunderFly-aerospace/TF-G2/
+#
+#
+
+. ${R}etc/init.d/rc.fw_defaults
+
+param set-default FW_AIRSPD_STALL 5
+
+param set-default FW_P_RMAX_NEG 20.0
+param set-default FW_W_RMAX 10
+param set-default FW_W_EN 1
+
+param set-default FW_RR_P 0.08
+
+param set-default MIS_LTRMIN_ALT 50
+param set-default MIS_TAKEOFF_ALT 7
+
+param set-default NAV_ACC_RAD 20
+param set-default NAV_DLL_ACT 2
+param set-default NAV_LOITER_RAD 50
+
+param set-default RWTO_TKOFF 0
+# Parameters related to autogyro takeoff PR
+#param set-default AG_TKOFF 1
+#param set-default AG_PROT_TYPE 1
+#param set-default AG_PROT_MIN_RPM 50.0
+#param set-default AG_PROT_TRG_RPM 900.0
+#param set-defoult AG_ROTOR_RPM 900.0
+
+param set-default FW_ARSP_SCALE_EN 0
+
+param set-default FW_AIRSPD_MAX 35
+param set-default FW_AIRSPD_MIN 7
+
+param set-default FW_P_LIM_MAX 25
+param set-default FW_P_LIM_MIN -5
+param set-default FW_R_LIM 30
+
+param set-default FW_MAN_P_MAX 30.0
+param set-default FW_MAN_R_MAX 30.0
+
+param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_IDLE 0
+param set-default COM_DISARM_PRFLT 0
+
+set MIXER_FILE etc/mixers-sitl/autogyro_sitl.main.mix
+set MIXER custom
@@ -78,6 +78,7 @@ px4_add_romfs_files(
 	3010_quadrotor_x
 	3011_hexarotor_x
 	17001_tf-g1
+	17002_tf-g2
 	2507_cloudship
 	6011_typhoon_h480
 	6011_typhoon_h480.post
@@ -29,9 +29,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_PR_P 0.2
 param set-default FW_RR_FF 0.6
@@ -1,22 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Quadplane VTOL
+# @name Generic Standard VTOL
 #
 # @type Standard VTOL
 # @class VTOL
-#
-# @maintainer
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-# @output AUX1 Aileron 1
-# @output AUX2 Aileron 2
-# @output AUX3 Elevator
-# @output AUX4 Rudder
-# @output AUX5 Throttle
-#
+
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@@ -24,21 +12,21 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2
 param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.15
-param set-default CA_ROTOR0_PY 0.15
-param set-default CA_ROTOR1_PX -0.15
-param set-default CA_ROTOR1_PY -0.15
-param set-default CA_ROTOR2_PX 0.15
-param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
 param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.15
-param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
 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 CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5
@@ -48,16 +36,5 @@ param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS3_TRQ_Y 1.0
 param set-default CA_SV_CS3_TYPE 4
-
-param set-default PWM_AUX_DIS5 950
-
 param set-default VT_TYPE 2
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-
 param set-default MAV_TYPE 22
-
-set MIXER quad_x
-set MIXER_AUX vtol_AAERT
-
-set PWM_OUT 1234
@@ -43,7 +43,7 @@ param set-default MPC_YAWRAUTO_MAX 40
 param set-default FW_PR_I 0.02
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_I 0.01
-param set-default FW_THR_CRUISE 0.75
+param set-default FW_THR_TRIM 0.75

 param set-default VT_ARSP_BLEND 6
 param set-default VT_ARSP_TRANS 12
@@ -14,7 +14,7 @@
 . ${R}etc/init.d/rc.vtol_defaults


-param set-default FW_THR_CRUISE 65
+param set-default FW_THR_TRIM 65
 param set-default FW_RR_FF 0.6

 param set-default MIS_YAW_TMT 10
@@ -53,7 +53,7 @@ param set-default FW_T_CLMB_MAX 3
 param set-default FW_T_SINK_MAX 3
 param set-default FW_T_SINK_MIN 1
 param set-default FW_T_VERT_ACC 6
-param set-default FW_THR_CRUISE 0.70
+param set-default FW_THR_TRIM 0.70
 param set-default FW_THR_SLEW_MAX 1
 param set-default FW_MAN_P_MAX 30
 param set-default FW_P_LIM_MAX 15
@@ -39,7 +39,7 @@ param set-default FW_RLL_TO_YAW_FF 0.1
 param set-default FW_RR_P 0.08
 param set-default FW_R_LIM 45
 param set-default FW_R_RMAX 50
-param set-default FW_THR_CRUISE 0.65
+param set-default FW_THR_TRIM 0.65
 param set-default FW_THR_MIN 0.3
 param set-default FW_THR_SLEW_MAX 0.6
 param set-default FW_T_HRATE_FF 0
@@ -0,0 +1,44 @@
+#!/bin/sh
+#
+# @name Generic Tiltrotor VTOL
+#
+# @type VTOL Tiltrotor
+# @class VTOL
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_TILT 2
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
+param set-default CA_ROTOR2_TILT 1
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+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 7
+param set-default CA_SV_CS2_TRQ_P 0.5
+param set-default CA_SV_CS2_TRQ_Y 0.5
+param set-default CA_SV_CS3_TYPE 8
+param set-default CA_SV_CS3_TRQ_P 0.5
+param set-default CA_SV_CS3_TRQ_Y -0.5
+param set-default CA_SV_TL_COUNT 2
+
+param set-default MAV_TYPE 21
+param set-default VT_TYPE 1
@@ -1,17 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Tailsitter
+# @name Generic VTOL Tailsitter
 #
-# @type VTOL Duo Tailsitter
+# @type VTOL Tailsitter
 # @class VTOL
 #
-# @output MAIN1 motor right
-# @output MAIN2 motor left
-# @output MAIN5 elevon right
-# @output MAIN6 elevon left
-#
-# @maintainer Roman Bapst <roman@px4.io>
-#
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@@ -19,18 +12,13 @@

 . ${R}etc/init.d/rc.vtol_defaults

-param set-default MAV_TYPE 19
-
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_MOT_COUNT 2
-param set-default VT_TYPE 0
-
+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
-param set-default CA_ROTOR0_PY 0.2
-param set-default CA_ROTOR1_KM -0.05
-param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR0_PY -0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PY 0.2
 param set-default CA_SV_CS_COUNT 2
 param set-default CA_SV_CS0_TRQ_P 0.5
 param set-default CA_SV_CS0_TRQ_Y 0.5
@@ -39,6 +27,6 @@ param set-default CA_SV_CS1_TRQ_P 0.5
 param set-default CA_SV_CS1_TRQ_Y -0.5
 param set-default CA_SV_CS1_TYPE 6

-set MIXER vtol_tailsitter_duo
-
-set PWM_OUT 1234
+param set-default MAV_TYPE 19
+param set-default VT_TYPE 0
+param set-default VT_ELEV_MC_LOCK 0
@@ -37,7 +37,7 @@ param set-default SENS_BOARD_ROT 8
 param set-default FW_AIRSPD_MAX 20
 param set-default FW_AIRSPD_MIN 7
 param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8

 param set-default FW_MAN_P_MAX 25
 param set-default FW_MAN_R_MAX 25
@@ -32,7 +32,7 @@ param set-default SENS_BOARD_ROT 4
 param set-default FW_AIRSPD_MAX 20
 param set-default FW_AIRSPD_MIN 7
 param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8

 param set-default FW_MAN_P_MAX 25
 param set-default FW_MAN_R_MAX 25
@@ -1,28 +1,16 @@
 #!/bin/sh
 #
-# @name Standard Plane
+# @name Generic Standard Plane
 #
 # @type Standard Plane
 # @class Plane
 #
-# @output MAIN1 aileron
-# @output MAIN2 elevator
-# @output MAIN3 throttle
-# @output MAIN4 rudder
-# @output MAIN5 flaps
-# @output MAIN6 gear
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
 # @board bitcraze_crazyflie exclude
 #

 . ${R}etc/init.d/rc.fw_defaults

+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
@@ -34,11 +22,3 @@ param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS3_TRQ_Y 1.0
 param set-default CA_SV_CS3_TYPE 4
-
-param set-default PWM_AUX_RATE 50
-param set-default PWM_MAIN_RATE 50
-
-set MIXER AETRFG
-
-# Rate must be set by group (see pwm info).
-# Throttle is in the same group as servos.
@@ -5,19 +5,20 @@
 # @type Flying Wing
 # @class Plane
 #
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer
-#
 # @board bitcraze_crazyflie exclude
 #

 . ${R}etc/init.d/rc.fw_defaults

-set MIXER fw_generic_wing
+param set-default SYS_CTRL_ALLOC 1
+param set-default CA_AIRFRAME 1
+
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 6
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
@@ -30,9 +30,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_P 0.04
@@ -32,9 +32,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_P 0.04
@@ -27,9 +27,6 @@ param set-default FW_AIRSPD_TRIM 16.5
 param set-default FW_L1_PERIOD 15
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 8
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 10
-param set-default FW_THR_LND_MAX 0
 param set-default FW_P_LIM_MAX 20
 param set-default FW_P_LIM_MIN -30
 param set-default FW_R_LIM 45
@@ -40,7 +37,7 @@ param set-default FW_RR_P 0.013
 param set-default FW_P_RMAX_NEG 70
 param set-default FW_P_RMAX_POS 70
 param set-default FW_R_RMAX 70
-param set-default FW_THR_CRUISE 0.55
+param set-default FW_THR_TRIM 0.55

 param set-default LNDFW_AIRSPD_MAX 6
 param set-default LNDFW_XYACC_MAX 4
@@ -124,6 +124,7 @@ px4_add_romfs_files(
 	13007_vtol_AAVVT_quad
 	13008_QuadRanger
 	13009_vtol_spt_ranger
+	13100_generic_vtol_tiltrotor
 	13012_convergence
 	13013_deltaquad
 	13014_vtol_babyshark
@@ -20,6 +20,7 @@ exec find boards msg src platforms test \
    -path src/modules/ekf2/EKF -prune -o \
    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
    -path src/modules/mavlink/mavlink -prune -o \
+    -path src/modules/mavlink/zenoh-pico -prune -o \
    -path src/modules/micrortps_bridge/micro-CDR -prune -o \
    -path src/modules/micrortps_bridge/microRTPS_client -prune -o \
    -path test/mavsdk_tests/catch2 -prune -o \
@@ -13,6 +13,7 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_BOSCH_BMI085=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
@@ -38,6 +39,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
@@ -46,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -61,7 +64,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
@@ -70,10 +72,9 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -8,8 +8,14 @@ board_adc start
 icm20602 -s -b 1 -R 8 start

 # Internal SPI bus BMI088 accel & gyro
-bmi088 -A -s -b 5 -R 8 start
-bmi088 -G -s -b 5 -R 8 start
+if bmi088 -A -s -b 5 -R 8 start
+then
+	bmi088 -G -s -b 5 -R 8 start
+else
+	# otherwise try BMI085
+	bmi085 -A -s -b 5 -R 8 start
+	bmi085 -G -s -b 5 -R 8 start
+fi

 # Internal ICM-20948 (with magnetometer)
 icm20948 -s -b 1 -R 8 -M start
@@ -47,6 +47,8 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI5, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
 		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
+		initSPIDevice(DRV_GYR_DEVTYPE_BMI085, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
+		initSPIDevice(DRV_ACC_DEVTYPE_BMI085, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
 	}),
 	initSPIBusExternal(SPI::Bus::SPI6, {
 		initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
@@ -0,0 +1,4 @@
+CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_CLIENT=y
+CONFIG_CYPHAL_APP_DESCRIPTOR=y
+CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y
@@ -4,14 +4,14 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
 CONFIG_DRIVERS_BOOTLOADERS=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_TFMINI=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_DRIVERS_PWM_OUT=y
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_CLIENT=y
-CONFIG_CYPHAL_APP_DESCRIPTOR=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y
+CONFIG_BOARD_UAVCAN_INTERFACES=1
+CONFIG_DRIVERS_UAVCANNODE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
@@ -23,5 +23,6 @@ CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -6,4 +6,9 @@
 pwm_out mode_pwm1 start

 ifup can0
-cyphal start
+
+# Start Cyphal when enabled
+if param compare -s CYPHAL_ENABLE 1
+then
+    cyphal start
+fi
@@ -112,6 +112,6 @@ CONFIG_SYMTAB_ORDEREDBYNAME=y
 CONFIG_SYSTEM_I2CTOOL=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_SPITOOL=y
-CONFIG_USER_ENTRYPOINT="nsh_main"
 CONFIG_USERMAIN_STACKSIZE=2176
+CONFIG_USER_ENTRYPOINT="nsh_main"
 CONFIG_WATCHDOG=y
@@ -12,6 +12,7 @@ fi
 if param compare -s ADC_ADS1115_EN 1
 then
 	ads1115 start -X
+	board_adc start -n
 else
 	board_adc start
 fi
@@ -4,6 +4,7 @@ CONFIG_DRIVERS_OSD=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
 CONFIG_SYSTEMCMDS_DUMPFILE=n
+CONFIG_SYSTEMCMDS_GPIO=n
 CONFIG_SYSTEMCMDS_MOTOR_TEST=n
 CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
@@ -40,6 +40,7 @@
 #include <px4_platform_common/tasks.h>
 #include <board_config.h>
 #include <stdio.h>
+#include <stdlib.h>

 #if defined(BOARD_HAS_POWER_CONTROL)
 int board_register_power_state_notification_cb(power_button_state_notification_t cb)
@@ -2,8 +2,10 @@ uint64 timestamp		# time since system start (microseconds)

 uint32 device_id                # unique device ID for the sensor that does not change between power cycles

-uint8 len                       # length of data
+uint16 len                       # length of data
 uint8 flags                     # LSB: 1=fragmented
 uint8[300] data                 # data to write to GPS device (RTCM message)

 uint8 ORB_QUEUE_LENGTH = 8
+
+uint8 MAX_INSTANCES = 2
@@ -1,9 +1,16 @@
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] time since system start
+float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing
+bool flaring # true if the aircraft is flaring

-float32 horizontal_slope_displacement
+# abort status is:
+# 0 if not aborted
+# >0 if aborted, with the singular abort criterion which triggered the landing abort enumerated by the following abort reasons
+uint8 abort_status

-float32 slope_angle_rad
-
-float32 flare_length
-
-bool abort_landing				# true if landing should be aborted
+# abort reasons
+# after the manual operator abort, corresponds to individual bits of param FW_LND_ABORT
+uint8 kNotAborted = 0
+uint8 kAbortedByOperator = 1
+uint8 kTerrainNotFound = 2 # FW_LND_ABORT (1 << 0)
+uint8 kTerrainTimeout = 3 # FW_LND_ABORT (1 << 1)
+uint8 kUnknownAbortCriterion = 4
@@ -40,6 +40,7 @@ int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation

 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
-float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
+bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
+float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover

 bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind
@@ -46,4 +46,7 @@ float32 heading			# heading angle of XYZ body frame rel to NED. Set to NaN if no
 float32 heading_offset		# heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
 float32 heading_accuracy	# heading accuracy (rad, [0, 2PI])

+float32 rtcm_injection_rate	# RTCM message injection rate Hz
+uint8 selected_rtcm_instance	# uorb instance that is being used for RTCM corrections
+
 # TOPICS sensor_gps vehicle_gps_position
@@ -40,5 +40,6 @@ float32 pitch_integ

 float32 throttle_sp
 float32 pitch_sp_rad
+float32 throttle_trim	# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions

 uint8 mode
@@ -253,10 +253,14 @@ void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, ui

 	while (!_should_exit_task) {
@[if recv_topics]@
-		while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
+
+		read = transport_node->read();
+		if (read > 0) {
 			total_rcvd += read;
 			rx_last_sec_read += read;
+		}

+		while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) {
 			uint64_t read_time = hrt_absolute_time();

 			switch (topic_ID) {
@@ -364,10 +364,14 @@ int main(int argc, char **argv)
 		if (!receiving) { start = std::chrono::steady_clock::now(); }

 		// Publish messages received from UART
-		if (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
+		length = transport_node->read();
+		if (length > 0) {
+			total_read += length;
+		}
+
+		while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) {
 			topics->publish(topic_ID, data_buffer, sizeof(data_buffer));
 			++received;
-			total_read += length;
 			receiving = true;
 			end = std::chrono::steady_clock::now();
 		}
@@ -113,14 +113,8 @@ uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len)
 	return crc;
 }

-ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer_len)
+ssize_t Transport_node::read()
 {
-	if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) {
-		return -1;
-	}
-
-	*topic_id = 255;
-
 	ssize_t len = node_read((void *)(_rx_buffer + _rx_buff_pos), sizeof(_rx_buffer) - _rx_buff_pos);

 	if (len < 0) {
@@ -143,85 +137,141 @@ ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer

 	_rx_buff_pos += len;

-	// We read some
-	size_t header_size = sizeof(struct Header);
+	return len;
+}

-	// but not enough
-	if (_rx_buff_pos < header_size) {
-		return 0;
+#ifndef PX4_DEBUG
+
+void Transport_node::print_buffer_debug()
+{
+	for (uint32_t i = 0; i < BUFFER_SIZE; ++i) {
+		if (i >= _rx_buff_pos) {
+			printf(".");
+			continue;
+		}
+
+		printf("%" PRIi8, _rx_buffer[i]);
+
+		if (_rx_buffer[i] == '>') {
+			printf("(X)");
+		}
+
+		printf(" ");
 	}

-	uint32_t msg_start_pos = 0;
+	printf("\n_rx_buff_pos: %" PRIu32 "\n", _rx_buff_pos);
+}

-	for (msg_start_pos = 0; msg_start_pos <= _rx_buff_pos - header_size; ++msg_start_pos) {
-		if ('>' == _rx_buffer[msg_start_pos] && memcmp(_rx_buffer + msg_start_pos, ">>>", 3) == 0) {
+#endif /* PX4_DEBUG */
+
+bool Transport_node::parse(
+	uint8_t *topic_id, char out_buffer[], size_t buffer_len)
+{
+	if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) {
+		return false;
+	}
+
+	*topic_id = 255;
+
+	static constexpr size_t header_size = sizeof(struct Header);
+
+	// No need to look for a message if not even a header fits into the buffer.
+	if (_rx_buff_pos < header_size) {
+		return false;
+	}
+
+	// Try to find the start of a message using the magic start sequence of '>>>'.
+	int32_t msg_start_pos = -1;
+
+	for (uint32_t i = 0; i <= _rx_buff_pos; ++i) {
+		if ('>' == _rx_buffer[i] && memcmp(_rx_buffer + i, ">>>", 3) == 0) {
+			msg_start_pos = i;
 			break;
 		}
 	}

-	// Start not found
-	if (msg_start_pos > (_rx_buff_pos - header_size)) {
+	auto DropDataBeforeIndex = [&](uint32_t index) {
+		if (index == 0) {
+			return;
+		}
+
+		if (index > _rx_buff_pos) {
+			index = _rx_buff_pos;
+
 #ifndef PX4_DEBUG

-		if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t                                (↓↓ %" PRIu32 ")\033[0m\n", msg_start_pos); }
+			if (_debug) { printf("\033[0;31m[ micrortps_transport ]\t clamped index from %" PRIu32 " to %" PRIu32 "\033[0m\n", index, _rx_buff_pos); }

 #else

-		if (_debug) { PX4_DEBUG("                               (↓↓ %" PRIu32 ")", msg_start_pos); }
+			if (_debug) { PX4_ERR("ERROR: clamped index from %" PRIu32 " to %" PRIu32 "\n", index, _rx_buff_pos); }

 #endif /* PX4_DEBUG */
+		}

-		// All we've checked so far is garbage, drop it - but save unchecked bytes
-		memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos);
-		_rx_buff_pos -= msg_start_pos;
-		return -1;
+		memmove(_rx_buffer, _rx_buffer + index, _rx_buff_pos - index);
+		_rx_buff_pos -= index;
+	};
+
+	// No start sequence has been found in the buffer. We can drop all data up to
+	// 2 characters before the end (to account for a non-complete start sequence).
+	if (msg_start_pos == -1) {
+		// Note, _rx_buff_pos should always be larger than 2 as we enforce a min.
+		// buffer content corresponding to a header length some lines above.
+		int32_t drop_before_idx = _rx_buff_pos - 2;
+		DropDataBeforeIndex(drop_before_idx);
+		return false;
 	}

-	// [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd]
-	struct Header *header = (struct Header *)&_rx_buffer[msg_start_pos];
-	uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l;
+	// A start sequence has been found. We can drop everything before this index
+	// in the buffer. This could happen at the startup when syncing to the
+	// stream or with noise on the data stream.
+	DropDataBeforeIndex(msg_start_pos);
+
+	// We have found a start sequence. Let's check if the header is in the buffer.
+	// [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart,
+	// ... ,payloadEnd]
+	if (header_size > _rx_buff_pos) {
+		// Header not yet in the buffer.
+		return false;
+	}
+
+	struct Header *header = (struct Header *)&_rx_buffer[0];
+
+	uint32_t payload_len =
+		((uint32_t)header->payload_len_h << 8) | header->payload_len_l;
+
+	// The message won't fit the output or processing buffer. This could happen
+	// with a corrupted header or by an actual message that is too large for our
+	// buffers. Let's drop it.
+	uint32_t message_length = header_size + payload_len;
+
+	if (message_length > buffer_len || (message_length + 3) > BUFFER_SIZE) {
+		DropDataBeforeIndex(3 + header_size);
+		return false;
+	}
+
+	// Let's check if all payload data according to the message length in the
+	// header is already in the buffer.
+	if ((header_size + payload_len) > _rx_buff_pos) {
+		// The buffer does not yet contain the full message. We need to wait for
+		// more data.
+		return false;
+	}

 	// The received message comes from this system. Discard it.
 	// This might happen when:
 	//   1. The same UDP port is being used to send a rcv packets or
-	//   2. The same topic on the agent is being used for outgoing and incoming data
+	//   2. The same topic on the agent is being used for outgoing and incoming
+	//   data
 	if (header->sys_id == _sys_id) {
-		// Drop the message and continue with the read buffer
-		memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1));
-		_rx_buff_pos -= (msg_start_pos + 1);
-		return -1;
-	}
-
-	// The message won't fit the buffer.
-	if (buffer_len < header_size + payload_len) {
-		// Drop the message and continue with the read buffer
-		memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1));
-		_rx_buff_pos -= (msg_start_pos + 1);
-		return -EMSGSIZE;
-	}
-
-	// We do not have a complete message yet
-	if (msg_start_pos + header_size + payload_len > _rx_buff_pos) {
-		// If there's garbage at the beginning, drop it
-		if (msg_start_pos > 0) {
-#ifndef PX4_DEBUG
-
-			if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t                                (↓ %" PRIu32 ")\033[0m\n", msg_start_pos); }
-
-#else
-
-			if (_debug) { PX4_DEBUG("                             (↓ %" PRIu32 ")", msg_start_pos); }
-
-#endif /* PX4_DEBUG */
-			memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos);
-			_rx_buff_pos -= msg_start_pos;
-		}
-
-		return 0;
+		DropDataBeforeIndex(3);
+		return false;
 	}

+	// Check the CRC of the message.
 	uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l;
-	uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + msg_start_pos + header_size, payload_len);
+	uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + header_size, payload_len);

 	if (read_crc != calc_crc) {
 #ifndef PX4_DEBUG
@@ -234,26 +284,16 @@ ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer

 #endif /* PX4_DEBUG */

-		// Drop garbage up just beyond the start of the message
-		memmove(_rx_buffer, _rx_buffer + (msg_start_pos + 1), _rx_buff_pos);
-
-		// If there is a CRC error, the payload len cannot be trusted
-		_rx_buff_pos -= (msg_start_pos + 1);
-
-		len = -1;
-
-	} else {
-		// copy message to outbuffer and set other return values
-		memmove(out_buffer, _rx_buffer + msg_start_pos + header_size, payload_len);
-		*topic_id = header->topic_id;
-		len = payload_len + header_size;
-
-		// discard message from _rx_buffer
-		_rx_buff_pos -= msg_start_pos + header_size + payload_len;
-		memmove(_rx_buffer, _rx_buffer + msg_start_pos + header_size + payload_len, _rx_buff_pos);
+		DropDataBeforeIndex(3);
+		return false;
 	}

-	return len;
+	// Copy message to output buffer and drop it from the buffer.
+	memmove(out_buffer, _rx_buffer + header_size, payload_len);
+	*topic_id = header->topic_id;
+
+	DropDataBeforeIndex(header_size + payload_len);
+	return true;
 }

 size_t Transport_node::get_header_length()
@@ -56,7 +56,9 @@ public:

 	virtual int init() {return 0;}
 	virtual uint8_t close() {return 0;}
-	ssize_t read(uint8_t *topic_id, char out_buffer[], size_t buffer_len);
+
+	ssize_t read();
+	bool parse(uint8_t *topic_id, char out_buffer[], size_t buffer_len);

 	/**
 	 * write a buffer
@@ -76,6 +78,10 @@ public:
 	size_t get_header_length();

 private:
+#ifndef PX4_DEBUG
+	void print_buffer_debug();
+#endif /* PX4_DEBUG */
+
 	struct __attribute__((packed)) Header {
 		char marker[3];
 		uint8_t topic_id;
@@ -64,6 +64,8 @@ float32 epv				# Standard deviation of vertical position error, (metres)
 float32 evh				# Standard deviation of horizontal velocity error, (metres/sec)
 float32 evv				# Standard deviation of horizontal velocity error, (metres/sec)

+bool dead_reckoning                     # True if this position is estimated through dead-reckoning
+
 # estimator specified vehicle limits
 float32 vxy_max				# maximum horizontal speed - set to 0 when limiting not required (meters/sec)
 float32 vz_max				# maximum vertical speed - set to 0 when limiting not required (meters/sec)
@@ -1,8 +1,9 @@
 uint64 timestamp	# time since system start (microseconds)

-float32 roll		# body angular rates in NED frame
-float32 pitch		# body angular rates in NED frame
-float32 yaw		# body angular rates in NED frame
+# body angular rates in NED frame
+float32 roll		# [rad/s] roll rate setpoint
+float32 pitch		# [rad/s] pitch rate setpoint
+float32 yaw		# [rad/s] yaw rate setpoint

 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
@@ -1,5 +1,11 @@
-uint64 timestamp				# time since system start (microseconds)
+# Encodes the system state of the vehicle published by commander

+uint64 timestamp # time since system start (microseconds)
+
+uint64 armed_time # Arming timestamp (microseconds)
+uint64 takeoff_time # Takeoff timestamp (microseconds)
+
+uint8 arming_state
 uint8 ARMING_STATE_INIT = 0
 uint8 ARMING_STATE_STANDBY = 1
 uint8 ARMING_STATE_ARMED = 2
@@ -8,93 +14,8 @@ uint8 ARMING_STATE_SHUTDOWN = 4
 uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6

-# FailureDetector status
-uint16 FAILURE_NONE = 0
-uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint16 FAILURE_ALT = 4               # (1 << 2)
-uint16 FAILURE_EXT = 8               # (1 << 3)
-uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
-uint16 FAILURE_MOTOR = 128           # (1 << 7)
-
-# HIL
-uint8 HIL_STATE_OFF = 0
-uint8 HIL_STATE_ON = 1
-
-# Navigation state, i.e. "what should vehicle do".
-uint8 NAVIGATION_STATE_MANUAL = 0		# Manual mode
-uint8 NAVIGATION_STATE_ALTCTL = 1		# Altitude control mode
-uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
-uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
-uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
-uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
-uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
-uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
-uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
-uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
-uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
-uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
-uint8 NAVIGATION_STATE_OFFBOARD = 14
-uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
-uint8 NAVIGATION_STATE_UNUSED2 = 16		# Free slot
-uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17	# Takeoff
-uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
-uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
-uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
-uint8 NAVIGATION_STATE_ORBIT = 21       # Orbit in a circle
-uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
-
-uint8 VEHICLE_TYPE_UNKNOWN = 0
-uint8 VEHICLE_TYPE_ROTARY_WING = 1
-uint8 VEHICLE_TYPE_FIXED_WING = 2
-uint8 VEHICLE_TYPE_ROVER = 3
-uint8 VEHICLE_TYPE_AIRSHIP = 4
-
-# state machine / state of vehicle.
-# Encodes the complete system state and is set by the commander app.
-
-uint8 nav_state				# set navigation state machine to specified value
-uint64 nav_state_timestamp              # time when current nav_state activated
-uint8 arming_state			# current arming state
-uint8 hil_state				# current hil state
-bool failsafe				# true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
-uint64 failsafe_timestamp               # time when failsafe was activated
-
-uint8 system_type			# system type, contains mavlink MAV_TYPE
-uint8 system_id			# system id, contains MAVLink's system ID field
-uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
-
-uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
-                            # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
-                            # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
-
-bool is_vtol				# True if the system is VTOL capable
-bool is_vtol_tailsitter                 # True if the system performs a 90° pitch down rotation during transition from MC to FW
-bool in_transition_mode			# True if VTOL is doing a transition
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
-
-bool rc_signal_lost				# true if RC reception lost
-
-bool data_link_lost				# datalink to GCS lost
-uint8 data_link_lost_counter			# counts unique data link lost events
-
-bool high_latency_data_link_lost 			# Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
-
-bool mission_failure				# Set to true if mission could not continue/finish
-bool geofence_violated
-
-uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
-
-# see SYS_STATUS mavlink message for the following
-# lower 32 bits are for the base flags, higher 32 bits are or the extended flags
-uint64 onboard_control_sensors_present
-uint64 onboard_control_sensors_enabled
-uint64 onboard_control_sensors_health
-
-
+uint8 latest_arming_reason
+uint8 latest_disarming_reason
 uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0
 uint8 ARM_DISARM_REASON_RC_STICK = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
@@ -110,12 +31,85 @@ uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11
 uint8 ARM_DISARM_REASON_SHUTDOWN = 12
 uint8 ARM_DISARM_REASON_UNIT_TEST = 13

-uint8 latest_arming_reason
-uint8 latest_disarming_reason
+uint64 nav_state_timestamp # time when current nav_state activated

-uint64 armed_time       # Arming timestamp (microseconds)
+# Navigation state "what should vehicle do"
+uint8 nav_state
+uint8 NAVIGATION_STATE_MANUAL = 0               # Manual mode
+uint8 NAVIGATION_STATE_ALTCTL = 1               # Altitude control mode
+uint8 NAVIGATION_STATE_POSCTL = 2               # Position control mode
+uint8 NAVIGATION_STATE_AUTO_MISSION = 3         # Auto mission mode
+uint8 NAVIGATION_STATE_AUTO_LOITER = 4          # Auto loiter mode
+uint8 NAVIGATION_STATE_AUTO_RTL = 5             # Auto return to launch mode
+uint8 NAVIGATION_STATE_UNUSED3 = 8              # Free slot
+uint8 NAVIGATION_STATE_UNUSED = 9               # Free slot
+uint8 NAVIGATION_STATE_ACRO = 10                # Acro mode
+uint8 NAVIGATION_STATE_UNUSED1 = 11             # Free slot
+uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position control)
+uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
+uint8 NAVIGATION_STATE_OFFBOARD = 14
+uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
+uint8 NAVIGATION_STATE_UNUSED2 = 16             # Free slot
+uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
+uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
+uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
+uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
+uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
+uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
+uint8 NAVIGATION_STATE_MAX = 23

-uint64 takeoff_time     # Takeoff timestamp (microseconds)
+# Bitmask of detected failures
+uint16 failure_detector_status
+uint16 FAILURE_NONE = 0
+uint16 FAILURE_ROLL = 1              # (1 << 0)
+uint16 FAILURE_PITCH = 2             # (1 << 1)
+uint16 FAILURE_ALT = 4               # (1 << 2)
+uint16 FAILURE_EXT = 8               # (1 << 3)
+uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
+uint16 FAILURE_BATTERY = 32          # (1 << 5)
+uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
+uint16 FAILURE_MOTOR = 128           # (1 << 7)
+
+uint8 hil_state
+uint8 HIL_STATE_OFF = 0
+uint8 HIL_STATE_ON = 1
+
+# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
+uint8 vehicle_type
+uint8 VEHICLE_TYPE_UNKNOWN = 0
+uint8 VEHICLE_TYPE_ROTARY_WING = 1
+uint8 VEHICLE_TYPE_FIXED_WING = 2
+uint8 VEHICLE_TYPE_ROVER = 3
+uint8 VEHICLE_TYPE_AIRSHIP = 4
+
+bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
+uint64 failsafe_timestamp # time when failsafe was activated
+
+# Link loss
+bool rc_signal_lost              # true if RC reception lost
+bool data_link_lost              # datalink to GCS lost
+uint8 data_link_lost_counter     # counts unique data link lost events
+bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
+
+# VTOL flags
+bool is_vtol             # True if the system is VTOL capable
+bool is_vtol_tailsitter  # True if the system performs a 90° pitch down rotation during transition from MC to FW
+bool in_transition_mode  # True if VTOL is doing a transition
+bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
+
+bool mission_failure # Set to true if mission could not continue/finish
+bool geofence_violated
+
+# MAVLink identification
+uint8 system_type  # system type, contains mavlink MAV_TYPE
+uint8 system_id	   # system id, contains MAVLink's system ID field
+uint8 component_id # subsystem / component id, contains MAVLink's component ID field
+
+# see SYS_STATUS mavlink message for the following
+# lower 32 bits are for the base flags, higher 32 bits are or the extended flags
+uint64 onboard_control_sensors_present
+uint64 onboard_control_sensors_enabled
+uint64 onboard_control_sensors_health

 bool safety_button_available # Set to true if a safety button is connected
 bool safety_off # Set to true if safety is off
@@ -29,7 +29,6 @@ bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_flight_termination_disabled
 bool circuit_breaker_engaged_usb_check
-bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled
 bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in fixed-wing mode should be allowed

 bool offboard_control_signal_lost
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+// minimal cmath
+
+#pragma once
+
+#include <cstdlib>
+#include <inttypes.h>
+
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace std
+{
+
+#if !defined(FLT_EPSILON)
+#define FLT_EPSILON     __FLT_EPSILON__
+#endif
+
+
+#ifdef isfinite
+#undef isfinite
+#endif  // isfinite
+
+inline bool isfinite(float value) { return __builtin_isfinite(value); }
+inline bool isfinite(double value) { return __builtin_isfinite(value); }
+inline bool isfinite(long double value) { return __builtin_isfinite(value); }
+
+
+#ifdef isnan
+#undef isnan
+#endif  // isnan
+
+inline bool isnan(float value) { return __builtin_isnan(value); }
+inline bool isnan(double value) { return __builtin_isnan(value); }
+inline bool isnan(long double value) { return __builtin_isnan(value); }
+
+
+#ifdef isinf
+#undef isinf
+#endif  // isinf
+
+inline bool isinf(float value) { return __builtin_isinf_sign(value); }
+inline bool isinf(double value) { return __builtin_isinf_sign(value); }
+inline bool isinf(long double value) { return __builtin_isinf_sign(value); }
+
+/*
+ * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually.
+ */
+#define NUTTX_WRAP_MATH_FUN_UNARY(name)                                   \
+	inline float       name(float x)       { return ::name##f(x); }          \
+	inline double      name(double x)      { return ::name(x); }             \
+	inline long double name(long double x) { return ::name##l(x); }
+
+#define NUTTX_WRAP_MATH_FUN_BINARY(name)                                                    \
+	inline float       name(float x, float y)             { return ::name##f(x, y); }          \
+	inline double      name(double x, double y)           { return ::name(x, y); }             \
+	inline long double name(long double x, long double y) { return ::name##l(x, y); }
+
+NUTTX_WRAP_MATH_FUN_UNARY(fabs)
+NUTTX_WRAP_MATH_FUN_UNARY(log)
+NUTTX_WRAP_MATH_FUN_UNARY(log10)
+NUTTX_WRAP_MATH_FUN_UNARY(exp)
+NUTTX_WRAP_MATH_FUN_UNARY(sqrt)
+NUTTX_WRAP_MATH_FUN_UNARY(sin)
+NUTTX_WRAP_MATH_FUN_UNARY(cos)
+NUTTX_WRAP_MATH_FUN_UNARY(tan)
+NUTTX_WRAP_MATH_FUN_UNARY(asin)
+NUTTX_WRAP_MATH_FUN_UNARY(acos)
+NUTTX_WRAP_MATH_FUN_UNARY(atan)
+NUTTX_WRAP_MATH_FUN_UNARY(sinh)
+NUTTX_WRAP_MATH_FUN_UNARY(cosh)
+NUTTX_WRAP_MATH_FUN_UNARY(tanh)
+NUTTX_WRAP_MATH_FUN_UNARY(ceil)
+NUTTX_WRAP_MATH_FUN_UNARY(floor)
+
+NUTTX_WRAP_MATH_FUN_BINARY(pow)
+NUTTX_WRAP_MATH_FUN_BINARY(atan2)
+
+}
@@ -38,10 +38,10 @@
 * and hardfault log support
 */

-#ifdef CONFIG_BOARD_CRASHDUMP
-
 #include <board_config.h>

+#ifdef CONFIG_BOARD_CRASHDUMP
+
 #include <stdio.h>
 #include <stdbool.h>
 #include <string.h>
@@ -31,6 +31,8 @@
 *
 ****************************************************************************/

+#include <board_config.h>
+
 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
 #include <arch/board/board.h>
@@ -41,6 +41,4 @@ size_t px4_get_secure_random(uint8_t *out,
 	arc4random_buf(out, outlen);
 	return outlen;
 }
-#else
-#error CONFIG_CRYPTO_RANDOM_POOL has to be defined
 #endif
@@ -40,3 +40,4 @@ add_subdirectory(../s32k1xx/hrt hrt)
 add_subdirectory(../s32k1xx/io_pins io_pins)
 add_subdirectory(../s32k1xx/tone_alarm tone_alarm)
 add_subdirectory(../s32k1xx/version version)
+add_subdirectory(../s32k1xx/watchdog watchdog)
@@ -39,6 +39,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <hardware/s32k1xx_rcm.h>
@@ -53,6 +54,26 @@ static int board_reset_enter_bootloader()
 	return OK;
 }

+static const uint32_t modes[] = {
+	/*                                      to  tb   */
+	/* BOARD_RESET_MODE_CLEAR                5   y   */  0,
+	/* BOARD_RESET_MODE_BOOT_TO_BL           0   n   */  0xb007b007,
+	/* BOARD_RESET_MODE_BOOT_TO_VALID_APP    0   y   */  0xb0070002,
+	/* BOARD_RESET_MODE_CAN_BL               10  n   */  0xb0080000,
+	/* BOARD_RESET_MODE_RTC_BOOT_FWOK        0   n   */  0xb0093a26
+};
+
+int board_configure_reset(reset_mode_e mode, uint32_t arg)
+{
+	int rv = -1;
+
+	if (mode < arraySize(modes)) {
+		//FIXME implemented this
+	}
+
+	return rv;
+}
+
 /****************************************************************************
 * Name: board_reset
 *
@@ -32,6 +32,8 @@
 ****************************************************************************/
 #pragma once

+#include <board_config.h>
+
 #if defined(CONFIG_SPI)

 #include "../../../stm32_common/include/px4_arch/spi_hw_description.h"
@@ -394,6 +394,7 @@ if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 		rascal
 		rascal-electric
 		tf-g1
+		tf-g2
 		tf-r1
 	)
 	set(all_posix_vmd_make_targets)
@@ -39,6 +39,7 @@
 #include <memory>
 #include <atomic>
 #include <pthread.h>
+#include <unistd.h>

 #include "lockstep_components.h"

@@ -39,8 +39,9 @@
 #include <nuttx/ioexpander/gpio.h>
 #endif

-ADC::ADC(uint32_t base_address, uint32_t channels) :
+ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_publish_adc_report(publish_adc_report),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
 	_base_address(base_address)
 {
@@ -116,7 +117,10 @@ void ADC::Run()
 		_samples[i].am_data = sample(_samples[i].am_channel);
 	}

-	update_adc_report(now);
+	if (_publish_adc_report) {
+		update_adc_report(now);
+	}
+
 	update_system_power(now);
 }

@@ -352,7 +356,8 @@ int ADC::custom_command(int argc, char *argv[])

 int ADC::task_spawn(int argc, char *argv[])
 {
-	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
+	bool publish_adc_report = !(argc >= 2 && strcmp(argv[1], "-n") == 0);
+	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report);

 	if (instance) {
 		_object.store(instance);
@@ -389,6 +394,7 @@ ADC driver.
 	PRINT_MODULE_USAGE_NAME("adc", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_COMMAND("test");
+	PRINT_MODULE_USAGE_PARAM_FLAG('n', "Do not publish ADC report, only system power", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

 	return 0;
@@ -64,7 +64,7 @@ using namespace time_literals;
 class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
 {
 public:
-	ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS);
+	ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS, bool publish_adc_report = true);

 	~ADC() override;

@@ -102,6 +102,8 @@ private:

 	static const hrt_abstime	kINTERVAL{10_ms};	/**< 100Hz base rate */

+	const bool 			_publish_adc_report;
+
 	perf_counter_t			_sample_perf;

 	unsigned			_channel_count{0};
@@ -110,6 +112,7 @@ private:

 	uORB::Publication<adc_report_s>		_to_adc_report{ORB_ID(adc_report)};
 	uORB::Publication<system_power_s>	_to_system_power{ORB_ID(system_power)};
+
 #ifdef BOARD_GPIO_VDD_5V_COMP_VALID
 	int _5v_comp_valid_fd {-1};
 #endif
@@ -58,6 +58,7 @@
 #include <lib/mixer_module/mixer_module.hpp>

 // UDRAL Specification Messages
+using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>

@@ -46,6 +46,8 @@
 #include <lib/parameters/param.h>
 #include <containers/List.hpp>

+#include <cmath>
+using std::isfinite;
 #include <uavcan/_register/Access_1_0.h>

 #include "../CanardHandle.hpp"
@@ -41,7 +41,7 @@
 * @reboot_required true
 * @group Cyphal
 */
-PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
+PARAM_DEFINE_INT32(CYPHAL_ENABLE, 1);

 /**
 * Cyphal Node ID.
@@ -53,6 +53,7 @@ extern "C" int sdp3x_main(int argc, char *argv[])
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_1_SDP3X;
 	cli.default_i2c_frequency = I2C_SPEED;
+	cli.support_keep_running = true;

 	const char *verb = cli.parseDefaultArguments(argc, argv);

@@ -137,7 +137,8 @@
 #define DRV_PWM_DEVTYPE_PCA9685     0x69
 #define DRV_ACC_DEVTYPE_BMI088		0x6a
 #define DRV_OSD_DEVTYPE_ATXXXX		0x6b
-
+#define DRV_ACC_DEVTYPE_BMI085		0x6C
+#define DRV_GYR_DEVTYPE_BMI085		0x6D

 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71
@@ -78,9 +78,11 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */

+using namespace time_literals;
+
 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
 #define TIMEOUT_5HZ		500		//!< Timeout time in mS,  200 mS (5Hz) + 300 mS delta for error
-#define RATE_MEASUREMENT_PERIOD 5000000
+#define RATE_MEASUREMENT_PERIOD 5_s

 enum class gps_driver_mode_t {
 	None = 0,
@@ -191,6 +193,8 @@ private:
 	unsigned			_last_rate_rtcm_injection_count{0};		///< counter for number of RTCM messages
 	unsigned			_num_bytes_read{0}; 				///< counter for number of read bytes from the UART (within update interval)
 	unsigned			_rate_reading{0}; 				///< reading rate in B/s
+	hrt_abstime			_last_rtcm_injection_time{0};			///< time of last rtcm injection
+	uint8_t				_selected_rtcm_instance{0};			///< uorb instance that is being used for RTCM corrections

 	const Instance 			_instance;

@@ -511,6 +515,24 @@ void GPS::handleInjectDataTopic()
 		return;
 	}

+	gps_inject_data_s msg;
+
+	// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
+	if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
+		_last_rtcm_injection_time = hrt_absolute_time();
+
+		for (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) {
+			if (_orb_inject_data_sub.ChangeInstance(i)) {
+				if (_orb_inject_data_sub.copy(&msg)) {
+					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
+						_selected_rtcm_instance = i;
+						break;
+					}
+				}
+			}
+		}
+	}
+
 	bool updated = false;

 	// Limit maximum number of GPS injections to 6 since usually
@@ -525,7 +547,6 @@ void GPS::handleInjectDataTopic()
 		updated = _orb_inject_data_sub.updated();

 		if (updated) {
-			gps_inject_data_s msg;

 			if (_orb_inject_data_sub.copy(&msg)) {

@@ -538,6 +559,7 @@ void GPS::handleInjectDataTopic()
 					injectData(msg.data, msg.len);

 					++_last_rate_rtcm_injection_count;
+					_last_rtcm_injection_time = hrt_absolute_time();
 				}
 			}
 		}
@@ -1143,6 +1165,9 @@ GPS::publish()
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
 		_report_gps_pos.device_id = get_device_id();

+		_report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance;
+		_report_gps_pos.rtcm_injection_rate = _rate_rtcm_injection;
+
 		_report_gps_pos_pub.publish(_report_gps_pos);
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "BMI085.hpp"
+
+#include "BMI085_Accelerometer.hpp"
+#include "BMI085_Gyroscope.hpp"
+
+I2CSPIDriverBase *BMI085::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	BMI085 *instance = nullptr;
+
+	if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI085) {
+		instance = new Bosch::BMI085::Accelerometer::BMI085_Accelerometer(config);
+
+	} else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI085) {
+		instance = new Bosch::BMI085::Gyroscope::BMI085_Gyroscope(config);
+	}
+
+	if (!instance) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (OK != instance->init()) {
+		delete instance;
+		return nullptr;
+	}
+
+	return instance;
+}
+
+BMI085::BMI085(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio)
+{
+}
+
+int BMI085::init()
+{
+	int ret = SPI::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("SPI::init failed (%i)", ret);
+		return ret;
+	}
+
+	return Reset() ? 0 : -1;
+}
+
+bool BMI085::Reset()
+{
+	_state = STATE::RESET;
+	ScheduleClear();
+	ScheduleNow();
+	return true;
+}
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/device/spi.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
+
+class BMI085 : public device::SPI, public I2CSPIDriver<BMI085>
+{
+public:
+	BMI085(const I2CSPIDriverConfig &config);
+
+	virtual ~BMI085() = default;
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	virtual void RunImpl() = 0;
+
+	int init() override;
+	virtual void print_status() = 0;
+
+protected:
+
+	bool Reset();
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	hrt_abstime _reset_timestamp{0};
+	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+	int _failure_count{0};
+
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
+	bool _data_ready_interrupt_enabled{false};
+
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		FIFO_READ,
+	} _state{STATE::RESET};
+
+	uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
+
+};
@@ -0,0 +1,604 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "BMI085_Accelerometer.hpp"
+
+#include <geo/geo.h> // CONSTANTS_ONE_G
+
+using namespace time_literals;
+
+namespace Bosch::BMI085::Accelerometer
+{
+
+BMI085_Accelerometer::BMI085_Accelerometer(const I2CSPIDriverConfig &config) :
+	BMI085(config),
+	_px4_accel(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
+	}
+
+	ConfigureSampleRate(_px4_accel.get_max_rate_hz());
+}
+
+BMI085_Accelerometer::~BMI085_Accelerometer()
+{
+	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(_drdy_missed_perf);
+}
+
+void BMI085_Accelerometer::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void BMI085_Accelerometer::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	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(_drdy_missed_perf);
+}
+
+int BMI085_Accelerometer::probe()
+{
+	/* 6.1 Serial Peripheral Interface (SPI)
+	 * ... the accelerometer part starts always in I2C mode
+	 * (regardless of the level of the PS pin) and needs to be changed to SPI
+	 *  mode actively by sending a rising edge on the CSB1 pin
+	 *  (chip select of the accelerometer), on which the accelerometer part
+	 *  switches to SPI mode and stays in this mode until the next power-up-reset.
+	 *
+	 *  To change the sensor to SPI mode in the initialization phase, the user
+	 *  could perfom a dummy SPI read operation, e.g. of register ACC_CHIP_ID
+	 *  (the obtained value will be invalid).In case of read operations,
+	 */
+	RegisterRead(Register::ACC_CHIP_ID);
+
+	const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
+
+	if (ACC_CHIP_ID != acc_chip_id) {
+		DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void BMI085_Accelerometer::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
+		RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value.
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if (RegisterRead(Register::ACC_CHIP_ID) == acc_chip_id) {
+			// ACC_PWR_CONF: Power on sensor
+			RegisterWrite(Register::ACC_PWR_CONF, 0);
+
+			// if reset succeeded then configure
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(10_ms);
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// 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
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_samples;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			if (samples == 0) {
+				// check current FIFO count
+				const uint16_t fifo_byte_counter = FIFOReadCount();
+
+				if (fifo_byte_counter >= FIFO::SIZE) {
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if ((fifo_byte_counter == 0) || (fifo_byte_counter == 0x8000)) {
+					// An empty FIFO corresponds to 0x8000
+					perf_count(_fifo_empty_perf);
+
+				} else {
+					samples = fifo_byte_counter / sizeof(FIFO::DATA);
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
+					if (samples > FIFO_MAX_SAMPLES) {
+						// not technically an overflow, but more samples than we expected or can publish
+						FIFOReset();
+						perf_count(_fifo_overflow_perf);
+						samples = 0;
+					}
+				}
+			}
+
+			bool success = false;
+
+			if (samples >= 1) {
+				if (FIFORead(timestamp_sample, samples)) {
+					success = true;
+
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void BMI085_Accelerometer::ConfigureAccel()
+{
+	const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
+
+	switch (ACC_RANGE) {
+	case acc_range_2g:
+		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
+		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
+		break;
+
+	case acc_range_4g:
+		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
+		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
+		break;
+
+	case acc_range_8g:
+		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
+		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
+		break;
+
+	case acc_range_16g:
+		_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f);
+		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
+		break;
+	}
+}
+
+void BMI085_Accelerometer::ConfigureSampleRate(int sample_rate)
+{
+	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+	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_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+
+	// recompute FIFO empty interval (us) with actual sample limit
+	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+
+	ConfigureFIFOWatermark(_fifo_samples);
+}
+
+void BMI085_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO_WTM: 13 bit FIFO watermark level value
+	// unit of the fifo watermark is one byte
+	const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
+
+	for (auto &r : _register_cfg) {
+		if (r.reg == Register::FIFO_WTM_0) {
+			// fifo_water_mark[7:0]
+			r.set_bits = fifo_watermark_threshold & 0x00FF;
+			r.clear_bits = ~r.set_bits;
+
+		} else if (r.reg == Register::FIFO_WTM_1) {
+			// fifo_water_mark[12:8]
+			r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8;
+			r.clear_bits = ~r.set_bits;
+		}
+	}
+}
+
+bool BMI085_Accelerometer::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureAccel();
+
+	return success;
+}
+
+int BMI085_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<BMI085_Accelerometer *>(arg)->DataReady();
+	return 0;
+}
+
+void BMI085_Accelerometer::DataReady()
+{
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
+}
+
+bool BMI085_Accelerometer::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool BMI085_Accelerometer::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+bool BMI085_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+uint8_t BMI085_Accelerometer::RegisterRead(Register reg)
+{
+	// 6.1.2 SPI interface of accelerometer part
+	//
+	// In case of read operations of the accelerometer part, the requested data
+	// is not sent immediately, but instead first a dummy byte is sent, and
+	// after this dummy byte the actual requested register content is transmitted.
+	uint8_t cmd[3] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	// cmd[1] dummy byte
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[2];
+}
+
+void BMI085_Accelerometer::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void BMI085_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+uint16_t BMI085_Accelerometer::FIFOReadCount()
+{
+	// FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
+	uint8_t fifo_len_buf[4] {};
+	fifo_len_buf[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ;
+	// fifo_len_buf[1] dummy byte
+
+	if (transfer(&fifo_len_buf[0], &fifo_len_buf[0], sizeof(fifo_len_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return 0;
+	}
+
+	const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2];        // fifo_byte_counter[7:0]
+	const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
+
+	return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
+}
+
+bool BMI085_Accelerometer::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);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
+
+	// An empty FIFO corresponds to 0x8000
+	if (fifo_byte_counter == 0x8000) {
+		perf_count(_fifo_empty_perf);
+		return false;
+
+	} else if (fifo_byte_counter >= FIFO::SIZE) {
+		perf_count(_fifo_overflow_perf);
+		return false;
+	}
+
+	sensor_accel_fifo_s accel{};
+	accel.timestamp_sample = timestamp_sample;
+	accel.samples = 0;
+	accel.dt = FIFO_SAMPLE_DT;
+
+	// first find all sensor data frames in the buffer
+	uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
+	unsigned fifo_buffer_index = 0; // start of buffer
+
+	while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
+		// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
+		switch (data_buffer[fifo_buffer_index] & 0xFC) {
+		case FIFO::header::sensor_data_frame: {
+				// Acceleration sensor data frame
+				// Frame length: 7 bytes (1 byte header + 6 bytes payload)
+
+				FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
+				const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
+				const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
+				const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
+
+				// 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[accel.samples] = accel_x;
+				accel.y[accel.samples] = math::negate(accel_y);
+				accel.z[accel.samples] = math::negate(accel_z);
+				accel.samples++;
+
+				fifo_buffer_index += 7; // move forward to next record
+			}
+			break;
+
+		case FIFO::header::skip_frame:
+			// Skip Frame
+			// Frame length: 2 bytes (1 byte header + 1 byte payload)
+			PX4_DEBUG("Skip Frame");
+			fifo_buffer_index += 2;
+			break;
+
+		case FIFO::header::sensor_time_frame:
+			// Sensortime Frame
+			// Frame length: 4 bytes (1 byte header + 3 bytes payload)
+			PX4_DEBUG("Sensortime Frame");
+			fifo_buffer_index += 4;
+			break;
+
+		case FIFO::header::FIFO_input_config_frame:
+			// FIFO input config Frame
+			// Frame length: 2 bytes (1 byte header + 1 byte payload)
+			PX4_DEBUG("FIFO input config Frame");
+			fifo_buffer_index += 2;
+			break;
+
+		case FIFO::header::sample_drop_frame:
+			// Sample drop Frame
+			// Frame length: 2 bytes (1 byte header + 1 byte payload)
+			PX4_DEBUG("Sample drop Frame");
+			fifo_buffer_index += 2;
+			break;
+
+		default:
+			fifo_buffer_index++;
+			break;
+		}
+	}
+
+	_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 (accel.samples > 0) {
+		_px4_accel.updateFIFO(accel);
+		return true;
+	}
+
+	return false;
+}
+
+void BMI085_Accelerometer::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E).
+	RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
+
+	// reset while FIFO is disabled
+	_drdy_timestamp_sample.store(0);
+}
+
+void BMI085_Accelerometer::UpdateTemperature()
+{
+	// stored in an 11-bit value in 2’s complement format
+	uint8_t temperature_buf[4] {};
+	temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | DIR_READ;
+	// temperature_buf[1] dummy byte
+
+	if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+	const uint8_t TEMP_MSB = temperature_buf[2];
+	const uint8_t TEMP_LSB = temperature_buf[3];
+
+	// Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data
+	uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32);
+	int16_t Temp_int11 = 0;
+
+	if (Temp_uint11 > 1023) {
+		Temp_int11 = Temp_uint11 - 2048;
+
+	} else {
+		Temp_int11 = Temp_uint11;
+	}
+
+	float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
+
+	if (PX4_ISFINITE(temperature)) {
+		_px4_accel.set_temperature(temperature);
+
+	} else {
+		perf_count(_bad_transfer_perf);
+	}
+}
+
+} // namespace Bosch::BMI085::Accelerometer
@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "BMI085.hpp"
+
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+
+#include "Bosch_BMI085_Accelerometer_Registers.hpp"
+
+namespace Bosch::BMI085::Accelerometer
+{
+
+class BMI085_Accelerometer : public BMI085
+{
+public:
+	BMI085_Accelerometer(const I2CSPIDriverConfig &config);
+	~BMI085_Accelerometer() override;
+
+	void RunImpl() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr uint32_t RATE{1600}; // 1600 Hz
+	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
+
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ};
+		uint8_t dummy{0};
+		uint8_t FIFO_LENGTH_0{0};
+		uint8_t FIFO_LENGTH_1{0};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_config_t {
+		Register reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Configure();
+	void ConfigureAccel();
+	void ConfigureSampleRate(int sample_rate = 0);
+	void ConfigureFIFOWatermark(uint8_t samples);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg);
+
+	uint8_t RegisterRead(Register reg);
+	void RegisterWrite(Register reg, uint8_t value);
+	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+	uint16_t FIFOReadCount();
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	void UpdateTemperature();
+
+	PX4Accelerometer _px4_accel;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+	uint8_t _checked_register{0};
+	static constexpr uint8_t size_register_cfg{10};
+	register_config_t _register_cfg[size_register_cfg] {
+		// Register                        | Set bits, Clear bits
+		{ Register::ACC_PWR_CONF,          0, ACC_PWR_CONF_BIT::acc_pwr_save },
+		{ Register::ACC_PWR_CTRL,          ACC_PWR_CTRL_BIT::acc_enable, 0 },
+		{ Register::ACC_CONF,              ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 },
+		{ Register::ACC_RANGE,             ACC_RANGE_BIT::acc_range_16g, 0 },
+		{ Register::FIFO_WTM_0,            0, 0 },
+		{ Register::FIFO_WTM_1,            0, 0 },
+		{ Register::FIFO_CONFIG_0,         FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 },
+		{ Register::FIFO_CONFIG_1,         FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 },
+		{ Register::INT1_IO_CONF,          INT1_IO_CONF_BIT::int1_out, 0 },
+		{ Register::INT1_INT2_MAP_DATA,    INT1_INT2_MAP_DATA_BIT::int1_fwm, 0},
+	};
+};
+
+} // namespace Bosch::BMI085::Accelerometer
@@ -0,0 +1,466 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "BMI085_Gyroscope.hpp"
+
+#include <px4_platform/board_dma_alloc.h>
+
+using namespace time_literals;
+
+namespace Bosch::BMI085::Gyroscope
+{
+
+BMI085_Gyroscope::BMI085_Gyroscope(const I2CSPIDriverConfig &config) :
+	BMI085(config),
+	_px4_gyro(get_device_id(), config.rotation)
+{
+	if (config.drdy_gpio != 0) {
+		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
+	}
+
+	ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
+}
+
+BMI085_Gyroscope::~BMI085_Gyroscope()
+{
+	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(_drdy_missed_perf);
+}
+
+void BMI085_Gyroscope::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void BMI085_Gyroscope::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
+
+	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(_drdy_missed_perf);
+}
+
+int BMI085_Gyroscope::probe()
+{
+	const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID);
+
+	if (chipid != ID) {
+		DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+void BMI085_Gyroscope::RunImpl()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	switch (_state) {
+	case STATE::RESET:
+		// 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);
+		_reset_timestamp = now;
+		_failure_count = 0;
+		_state = STATE::WAIT_FOR_RESET;
+		ScheduleDelayed(30_ms);
+		break;
+
+	case STATE::WAIT_FOR_RESET:
+		if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) {
+			// if reset succeeded then configure
+			_state = STATE::CONFIGURE;
+			ScheduleDelayed(1_ms);
+
+		} else {
+			// RESET not complete
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Reset failed, retrying");
+				_state = STATE::RESET;
+				ScheduleDelayed(100_ms);
+
+			} else {
+				PX4_DEBUG("Reset not complete, check again in 10 ms");
+				ScheduleDelayed(10_ms);
+			}
+		}
+
+		break;
+
+	case STATE::CONFIGURE:
+		if (Configure()) {
+			// 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
+			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+				PX4_DEBUG("Configure failed, resetting");
+				_state = STATE::RESET;
+
+			} else {
+				PX4_DEBUG("Configure failed, retrying");
+			}
+
+			ScheduleDelayed(100_ms);
+		}
+
+		break;
+
+	case STATE::FIFO_READ: {
+			hrt_abstime timestamp_sample = 0;
+
+			if (_data_ready_interrupt_enabled) {
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+
+				} else {
+					perf_count(_drdy_missed_perf);
+				}
+
+				// push backup schedule back
+				ScheduleDelayed(_fifo_empty_interval_us * 2);
+			}
+
+			// always check current FIFO status/count
+			bool success = false;
+			const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
+
+			if (FIFO_STATUS & FIFO_STATUS_BIT::Fifo_overrun) {
+				FIFOReset();
+				perf_count(_fifo_overflow_perf);
+
+			} else {
+				const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::Fifo_frame_counter;
+
+				if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
+					// not technically an overflow, but more samples than we expected or can publish
+					FIFOReset();
+					perf_count(_fifo_overflow_perf);
+
+				} else if (fifo_frame_counter == 0) {
+					perf_count(_fifo_empty_perf);
+
+				} else if (fifo_frame_counter >= 1) {
+
+					uint8_t samples = fifo_frame_counter;
+
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_samples + 1) {
+						// sample timestamp set from data ready already corresponds to _fifo_samples
+						if (timestamp_sample == 0) {
+							timestamp_sample = now - static_cast<int>(FIFO_SAMPLE_DT);
+						}
+
+						samples--;
+					}
+
+					if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) {
+						success = true;
+
+						if (_failure_count > 0) {
+							_failure_count--;
+						}
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					return;
+				}
+			}
+
+			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
+				// check configuration registers periodically or immediately following any failure
+				if (RegisterCheck(_register_cfg[_checked_register])) {
+					_last_config_check_timestamp = now;
+					_checked_register = (_checked_register + 1) % size_register_cfg;
+
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
+			}
+		}
+
+		break;
+	}
+}
+
+void BMI085_Gyroscope::ConfigureGyro()
+{
+	const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
+
+	switch (GYRO_RANGE) {
+	case gyro_range_2000_dps:
+		_px4_gyro.set_scale(math::radians(1.f / 16.384f));
+		_px4_gyro.set_range(math::radians(2000.f));
+		break;
+
+	case gyro_range_1000_dps:
+		_px4_gyro.set_scale(math::radians(1.f / 32.768f));
+		_px4_gyro.set_range(math::radians(1000.f));
+		break;
+
+	case gyro_range_500_dps:
+		_px4_gyro.set_scale(math::radians(1.f / 65.536f));
+		_px4_gyro.set_range(math::radians(500.f));
+		break;
+
+	case gyro_range_250_dps:
+		_px4_gyro.set_scale(math::radians(1.f / 131.072f));
+		_px4_gyro.set_range(math::radians(250.f));
+		break;
+
+	case gyro_range_125_dps:
+		_px4_gyro.set_scale(math::radians(1.f / 262.144f));
+		_px4_gyro.set_range(math::radians(125.f));
+		break;
+	}
+}
+
+void BMI085_Gyroscope::ConfigureSampleRate(int sample_rate)
+{
+	// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
+	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_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
+
+	// recompute FIFO empty interval (us) with actual sample limit
+	_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
+
+	ConfigureFIFOWatermark(_fifo_samples);
+}
+
+void BMI085_Gyroscope::ConfigureFIFOWatermark(uint8_t samples)
+{
+	// FIFO watermark threshold
+	for (auto &r : _register_cfg) {
+		if (r.reg == Register::FIFO_CONFIG_0) {
+			r.set_bits = samples;
+			r.clear_bits = ~r.set_bits;
+		}
+	}
+}
+
+bool BMI085_Gyroscope::Configure()
+{
+	// first set and clear all configured register bits
+	for (const auto &reg_cfg : _register_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
+
+	// now check that all are configured
+	bool success = true;
+
+	for (const auto &reg_cfg : _register_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
+
+	ConfigureGyro();
+
+	return success;
+}
+
+int BMI085_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<BMI085_Gyroscope *>(arg)->DataReady();
+	return 0;
+}
+
+void BMI085_Gyroscope::DataReady()
+{
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
+}
+
+bool BMI085_Gyroscope::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
+}
+
+bool BMI085_Gyroscope::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+bool BMI085_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
+{
+	bool success = true;
+
+	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
+
+	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
+		success = false;
+	}
+
+	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
+		success = false;
+	}
+
+	return success;
+}
+
+uint8_t BMI085_Gyroscope::RegisterRead(Register reg)
+{
+	uint8_t cmd[2] {};
+	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
+	transfer(cmd, cmd, sizeof(cmd));
+	return cmd[1];
+}
+
+void BMI085_Gyroscope::RegisterWrite(Register reg, uint8_t value)
+{
+	uint8_t cmd[2] { (uint8_t)reg, value };
+	transfer(cmd, cmd, sizeof(cmd));
+}
+
+void BMI085_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
+{
+	const uint8_t orig_val = RegisterRead(reg);
+
+	uint8_t val = (orig_val & ~clearbits) | setbits;
+
+	if (orig_val != val) {
+		RegisterWrite(reg, val);
+	}
+}
+
+bool BMI085_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
+{
+	FIFOTransferBuffer buffer{};
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
+
+	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
+		perf_count(_bad_transfer_perf);
+		return false;
+	}
+
+	sensor_gyro_fifo_s gyro{};
+	gyro.timestamp_sample = timestamp_sample;
+	gyro.samples = samples;
+	gyro.dt = FIFO_SAMPLE_DT;
+
+	for (int i = 0; i < samples; i++) {
+		const FIFO::DATA &fifo_sample = buffer.f[i];
+
+		const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
+		const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
+		const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
+
+		// 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;
+		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
+		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+	}
+
+	_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));
+
+	_px4_gyro.updateFIFO(gyro);
+
+	return true;
+}
+
+void BMI085_Gyroscope::FIFOReset()
+{
+	perf_count(_fifo_reset_perf);
+
+	// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
+	RegisterWrite(Register::FIFO_CONFIG_0, 0);
+
+	// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
+	RegisterWrite(Register::FIFO_CONFIG_1, 0);
+
+	// reset while FIFO is disabled
+	_drdy_timestamp_sample.store(0);
+
+	// FIFO_CONFIG_0: restore FIFO watermark
+	// FIFO_CONFIG_1: re-enable FIFO
+	for (const auto &r : _register_cfg) {
+		if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
+			RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
+		}
+	}
+}
+
+} // namespace Bosch::BMI085::Gyroscope
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "BMI085.hpp"
+
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+
+#include "Bosch_BMI085_Gyroscope_Registers.hpp"
+
+namespace Bosch::BMI085::Gyroscope
+{
+
+class BMI085_Gyroscope : public BMI085
+{
+public:
+	BMI085_Gyroscope(const I2CSPIDriverConfig &config);
+	~BMI085_Gyroscope() override;
+
+	void RunImpl() override;
+	void print_status() override;
+
+private:
+	void exit_and_cleanup() override;
+
+	// Sensor Configuration
+	static constexpr uint32_t RATE{2000}; // 2000 Hz
+	static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
+
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))};
+
+	// Transfer data
+	struct FIFOTransferBuffer {
+		uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA) | DIR_READ};
+		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
+	};
+	// ensure no struct padding
+	static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+
+	struct register_config_t {
+		Register reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
+	int probe() override;
+
+	bool Configure();
+	void ConfigureGyro();
+	void ConfigureSampleRate(int sample_rate = 0);
+	void ConfigureFIFOWatermark(uint8_t samples);
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	bool RegisterCheck(const register_config_t &reg_cfg);
+
+	uint8_t RegisterRead(Register reg);
+	void RegisterWrite(Register reg, uint8_t value);
+	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
+
+	bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
+	void FIFOReset();
+
+	PX4Gyroscope _px4_gyro;
+
+	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
+	perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
+	perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")};
+	perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")};
+	perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")};
+	perf_counter_t _drdy_missed_perf{nullptr};
+
+	uint8_t _fifo_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / RATE))};
+
+	uint8_t _checked_register{0};
+	static constexpr uint8_t size_register_cfg{8};
+	register_config_t _register_cfg[size_register_cfg] {
+		// Register                         | Set bits, Clear bits
+		{ Register::GYRO_RANGE,             GYRO_RANGE_BIT::gyro_range_2000_dps, 0 },
+		{ Register::GYRO_BANDWIDTH,         0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz },
+		{ Register::GYRO_INT_CTRL,          GYRO_INT_CTRL_BIT::fifo_en, 0 },
+		{ Register::INT3_INT4_IO_CONF,      0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl },
+		{ Register::INT3_INT4_IO_MAP,       INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 },
+		{ Register::FIFO_WM_ENABLE,         FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
+		{ Register::FIFO_CONFIG_0,          0, 0 }, // fifo_water_mark_level_trigger_retain<6:0>
+		{ Register::FIFO_CONFIG_1,          FIFO_CONFIG_1_BIT::FIFO_MODE, 0 },
+	};
+};
+
+} // namespace Bosch::BMI085::Gyroscope
@@ -0,0 +1,171 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace Bosch::BMI085::Accelerometer
+{
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t acc_chip_id = 0x1F;
+
+enum class Register : uint8_t {
+	ACC_CHIP_ID        = 0x00,
+
+	TEMP_MSB           = 0x22,
+	TEMP_LSB           = 0x23,
+
+	FIFO_LENGTH_0      = 0x24,
+	FIFO_LENGTH_1      = 0x25,
+
+	FIFO_DATA          = 0x26,
+
+	ACC_CONF           = 0x40,
+	ACC_RANGE          = 0x41,
+
+	FIFO_WTM_0         = 0x46,
+	FIFO_WTM_1         = 0x47,
+	FIFO_CONFIG_0      = 0x48,
+	FIFO_CONFIG_1      = 0x49,
+
+	INT1_IO_CONF       = 0x53,
+
+	INT1_INT2_MAP_DATA = 0x58,
+
+	ACC_PWR_CONF       = 0x7C,
+	ACC_PWR_CTRL       = 0x7D,
+	ACC_SOFTRESET      = 0x7E,
+};
+
+// ACC_CONF
+enum ACC_CONF_BIT : uint8_t {
+	// [7:4] acc_bwp
+	acc_bwp_Normal = Bit7 | Bit5,        // 0x0a (0b1010) Normal
+
+	// [3:0] acc_odr
+	acc_odr_1600   = Bit3 | Bit2,        // 0x0C ODR 1600 Hz
+};
+
+// ACC_RANGE
+enum ACC_RANGE_BIT : uint8_t {
+	acc_range_2g  = 0,           // ±2g
+	acc_range_4g  = Bit0,        // ±4g
+	acc_range_8g  = Bit1,        // ±8g
+	acc_range_16g = Bit1 | Bit0, // ±16g
+};
+
+// FIFO_CONFIG_0
+enum FIFO_CONFIG_0_BIT : uint8_t {
+	BIT1_ALWAYS = Bit1, // This bit must always be ‘1’.
+	FIFO_mode   = Bit0, // FIFO mode
+};
+
+// FIFO_CONFIG_1
+enum FIFO_CONFIG_1_BIT : uint8_t {
+	Acc_en      = Bit6,
+	BIT4_ALWAYS = Bit4, // This bit must always be ‘1’.
+};
+
+// INT1_IO_CONF
+enum INT1_IO_CONF_BIT : uint8_t {
+	int1_in  = Bit4,
+	int1_out = Bit3,
+
+	int1_lvl = Bit1,
+};
+
+// INT1_INT2_MAP_DATA
+enum INT1_INT2_MAP_DATA_BIT : uint8_t {
+	int2_drdy  = Bit6,
+	int2_fwm   = Bit5,
+	int2_ffull = Bit4,
+
+	int1_drdy  = Bit2,
+	int1_fwm   = Bit1,
+	int1_ffull = Bit0,
+};
+
+// ACC_PWR_CONF
+enum ACC_PWR_CONF_BIT : uint8_t {
+	acc_pwr_save = 0x03
+};
+
+// ACC_PWR_CTRL
+enum ACC_PWR_CTRL_BIT : uint8_t {
+	acc_enable = 0x04
+};
+
+namespace FIFO
+{
+static constexpr size_t SIZE = 1024;
+
+// 1. Acceleration sensor data frame   - Frame length: 7 bytes (1 byte header + 6 bytes payload)
+// Payload: the next bytes contain the sensor data in the same order as defined in the register map (addresses 0x12 – 0x17).
+// 2. Skip Frame                       - Frame length: 2 bytes (1 byte header + 1 byte payload)
+// Payload: one byte containing the number of skipped frames. When more than 0xFF frames have been skipped, 0xFF is returned.
+// 3. Sensortime Frame                 - Frame length: 4 bytes (1 byte header + 3 bytes payload)
+// Payload: Sensortime (content of registers 0x18 – 0x1A), taken when the last byte of the last frame is read.
+
+struct DATA {
+	uint8_t Header;
+	uint8_t ACC_X_LSB;
+	uint8_t ACC_X_MSB;
+	uint8_t ACC_Y_LSB;
+	uint8_t ACC_Y_MSB;
+	uint8_t ACC_Z_LSB;
+	uint8_t ACC_Z_MSB;
+};
+static_assert(sizeof(DATA) == 7);
+
+enum header : uint8_t {
+	sensor_data_frame       = 0b10000100,
+	skip_frame              = 0b01000000,
+	sensor_time_frame       = 0b01000100,
+	FIFO_input_config_frame = 0b01001000,
+	sample_drop_frame       = 0b01010000,
+};
+
+} // namespace FIFO
+} // namespace Bosch::BMI085::Accelerometer
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace Bosch::BMI085::Gyroscope
+{
+
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
+static constexpr uint8_t DIR_READ = 0x80;
+
+static constexpr uint8_t ID = 0x0F;
+
+enum class Register : uint8_t {
+	GYRO_CHIP_ID      = 0x00,
+
+	FIFO_STATUS       = 0x0E,
+	GYRO_RANGE        = 0x0F,
+	GYRO_BANDWIDTH    = 0x10,
+
+	GYRO_SOFTRESET    = 0x14,
+	GYRO_INT_CTRL     = 0x15,
+	INT3_INT4_IO_CONF = 0x16,
+	INT3_INT4_IO_MAP  = 0x18,
+
+	FIFO_WM_ENABLE    = 0x1E,
+
+	FIFO_CONFIG_0     = 0x3D,
+	FIFO_CONFIG_1     = 0x3E,
+	FIFO_DATA         = 0x3F,
+};
+
+// FIFO_STATUS
+enum FIFO_STATUS_BIT : uint8_t {
+	Fifo_overrun       = Bit7,
+	Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
+};
+
+// GYRO_RANGE
+enum GYRO_RANGE_BIT : uint8_t {
+	gyro_range_2000_dps = 0x00, // ±2000
+	gyro_range_1000_dps = 0x01, // ±1000
+	gyro_range_500_dps  = 0x02, // ±500
+	gyro_range_250_dps  = 0x04, // ±250
+	gyro_range_125_dps  = 0x05, // ±125
+};
+
+// GYRO_BANDWIDTH
+enum GYRO_BANDWIDTH_BIT : uint8_t {
+	gyro_bw_532_Hz = Bit2 | Bit1 | Bit0
+};
+
+// GYRO_INT_CTRL
+enum GYRO_INT_CTRL_BIT : uint8_t {
+	data_en = Bit7,
+	fifo_en = Bit6,
+};
+
+// INT3_INT4_IO_CONF
+enum INT3_INT4_IO_CONF_BIT : uint8_t {
+	Int3_od  = Bit1, // ‘0’ Push-pull
+	Int3_lvl = Bit0, // ‘0’ Active low
+};
+
+// INT3_INT4_IO_MAP
+enum INT3_INT4_IO_MAP_BIT : uint8_t {
+	Int4_data = Bit7,
+	Int4_fifo = Bit5,
+	Int3_fifo = Bit2,
+	Int3_data = Bit0,
+};
+
+// FIFO_WM_ENABLE
+enum FIFO_WM_ENABLE_BIT : uint8_t {
+	fifo_wm_enable  = Bit7 | Bit3,
+	fifo_wm_disable = Bit3,
+};
+
+// FIFO_CONFIG_1
+enum FIFO_CONFIG_1_BIT : uint8_t {
+	FIFO_MODE = Bit6,
+};
+
+
+namespace FIFO
+{
+struct DATA {
+	uint8_t RATE_X_LSB;
+	uint8_t RATE_X_MSB;
+	uint8_t RATE_Y_LSB;
+	uint8_t RATE_Y_MSB;
+	uint8_t RATE_Z_LSB;
+	uint8_t RATE_Z_MSB;
+};
+static_assert(sizeof(DATA) == 6);
+
+// 100 frames of data in FIFO mode
+static constexpr size_t SIZE = sizeof(DATA) * 100;
+
+} // namespace FIFO
+} // namespace Bosch::BMI085::Gyroscope
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022 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
@@ -31,4 +31,24 @@
 #
 ############################################################################

-px4_add_library(landing_slope Landingslope.cpp)
+px4_add_module(
+	MODULE drivers__imu__bosch__bmi085
+	MAIN bmi085
+	COMPILE_FLAGS
+	SRCS
+		Bosch_BMI085_Accelerometer_Registers.hpp
+		Bosch_BMI085_Gyroscope_Registers.hpp
+
+		BMI085.cpp
+		BMI085.hpp
+		BMI085_Accelerometer.cpp
+		BMI085_Accelerometer.hpp
+		BMI085_Gyroscope.cpp
+		BMI085_Gyroscope.hpp
+
+		bmi085_main.cpp
+	DEPENDS
+		drivers_accelerometer
+		drivers_gyroscope
+		px4_work_queue
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_IMU_BOSCH_BMI085
+	bool "bosch bmi085"
+	default n
+	---help---
+		Enable support for bosch bmi085
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "BMI085.hpp"
+
+void BMI085::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("bmi085", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("imu");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int bmi085_main(int argc, char *argv[])
+{
+	int ch;
+	using ThisDriver = BMI085;
+	BusCLIArguments cli{false, true};
+	uint16_t type = 0;
+	cli.default_spi_frequency = 10000000;
+	const char *name = MODULE_NAME;
+
+	while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) {
+		switch (ch) {
+		case 'A':
+			type = DRV_ACC_DEVTYPE_BMI085;
+			name = MODULE_NAME "_accel";
+			break;
+
+		case 'G':
+			type = DRV_GYR_DEVTYPE_BMI085;
+			name = MODULE_NAME "_gyro";
+			break;
+
+		case 'R':
+			cli.rotation = (enum Rotation)atoi(cli.optArg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+
+	if (!verb || type == 0) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(name, cli, type);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstddef>

 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstddef>

 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstddef>

 namespace InvenSense_ICM20948
 {
@@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstddef>

 namespace InvenSense_ICM42688P
 {
@@ -63,13 +63,21 @@ int RM3100::self_test()
 {
 	bool complete = false;

-	uint8_t cmd = (CMM_DEFAULT | POLLING_MODE);
+	// Set the default command mode and enable polling (not continuous mode)
+	uint8_t cmd = (CMM_DEFAULT & ~CONTINUOUS_MODE);
 	int ret = _interface->write(ADDR_CMM, &cmd, 1);

 	if (ret != PX4_OK) {
 		return ret;
 	}

+	cmd = HSHAKE_NO_DRDY_CLEAR;
+	ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
+
+	if (ret != PX4_OK) {
+		return ret;
+	}
+
 	// Configure sensor to execute BIST upon receipt of a POLL command
 	cmd = BIST_SELFTEST;
 	ret = _interface->write(ADDR_BIST, &cmd, 1);
@@ -78,27 +86,20 @@ int RM3100::self_test()
 		return ret;
 	}

-	/* Perform test procedure until a valid result is obtained or test times out */
+	// Poll to start the self test
+	cmd = POLL_XYZ;
+	ret = _interface->write(ADDR_POLL, &cmd, 1);
+
+	if (ret != PX4_OK) {
+		return ret;
+	}
+
+	// Perform test procedure until a valid result is obtained or test times out
+
 	const hrt_abstime t_start = hrt_absolute_time();

 	while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {

-		// Re-disable DRDY clear
-		cmd = HSHAKE_NO_DRDY_CLEAR;
-		ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
-
-		if (ret != PX4_OK) {
-			return ret;
-		}
-
-		// Poll for a measurement
-		cmd = POLL_XYZ;
-		ret = _interface->write(ADDR_POLL, &cmd, 1);
-
-		if (ret != PX4_OK) {
-			return ret;
-		}
-
 		uint8_t status = 0;
 		ret = _interface->read(ADDR_STATUS, &status, 1);

@@ -119,8 +120,16 @@ int RM3100::self_test()
 			if (cmd & BIST_STE) {
 				complete = true;

-				// If the test passed, disable self-test mode by clearing the STE bit
-				if (cmd & BIST_XYZ_OK) {
+				// If the x, y, or z LR oscillators malfunctioned then the self test failed.
+				if ((cmd & BIST_XYZ_OK) ^ BIST_XYZ_OK) {
+					PX4_ERR("built-in self test failed: 0x%2X x:%s y:%s z:%s", cmd,
+						cmd & 0x10 ? "Pass" : "Fail",
+						cmd & 0x20 ? "Pass" : "Fail",
+						cmd & 0x40 ? "Pass" : "Fail");
+					return PX4_ERROR;
+
+				} else {
+					// The test passed, disable self-test mode by clearing the STE bit
 					cmd = 0;
 					ret = _interface->write(ADDR_BIST, &cmd, 1);

@@ -129,9 +138,6 @@ int RM3100::self_test()
 					}

 					return PX4_OK;
-
-				} else {
-					PX4_ERR("built-in self test failed");
 				}
 			}
 		}
@@ -261,6 +267,9 @@ int RM3100::set_default_register_values()
 	cmd[1] = CCZ_DEFAULT_LSB;
 	_interface->write(ADDR_CCZ, cmd, 2);

+	cmd[0] = HSHAKE_DEFAULT;
+	_interface->write(ADDR_HSHAKE, cmd, 1);
+
 	cmd[0] = CMM_DEFAULT;
 	_interface->write(ADDR_CMM, cmd, 1);

@@ -531,11 +531,6 @@ void PX4IO::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);

-	// schedule minimal update rate if there are no actuator controls
-	if (!_mixing_output.useDynamicMixing()) {
-		ScheduleDelayed(20_ms);
-	}
-
 	/* if we have new control data from the ORB, handle it */
 	if (_param_sys_hitl.get() <= 0) {
 		_mixing_output.update();
@@ -654,6 +649,9 @@ void PX4IO::Run()

 	_mixing_output.updateSubscriptions(true, true);

+	// minimal backup scheduling
+	ScheduleDelayed(20_ms);
+
 	perf_end(_cycle_perf);
 }

@@ -1,7 +1,6 @@
 menu "stub_keystore configuration"
 	menuconfig DRIVERS_STUB_KEYSTORE
 	bool "stub_keystore"
-	depends on DRIVERS_SW_CRYPTO
 	default n
 	---help---
 		Enable support for stub_keystore
@@ -4,5 +4,4 @@ serial_config:
      port_config_param:
        name: MXS_SER_CFG
        group: Transponder
-        default: TEL2
      label: Sagetech MXS Serial Port
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2022 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
@@ -46,6 +46,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
+#include <lib/parameters/param.h>

 using namespace time_literals;

@@ -57,9 +58,9 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 	_sub_auxiliary(node),
 	_sub_fix(node),
 	_sub_fix2(node),
-	_pub_rtcm(node),
-	_channel_using_fix2(new bool[_max_channels]),
-	_rtcm_perf(perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm pub"))
+	_pub_moving_baseline_data(node),
+	_pub_rtcm_stream(node),
+	_channel_using_fix2(new bool[_max_channels])
 {
 	for (uint8_t i = 0; i < _max_channels; i++) {
 		_channel_using_fix2[i] = false;
@@ -71,7 +72,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 UavcanGnssBridge::~UavcanGnssBridge()
 {
 	delete [] _channel_using_fix2;
-	perf_free(_rtcm_perf);
+	perf_free(_rtcm_stream_pub_perf);
+	perf_free(_moving_baseline_data_pub_perf);
 }

 int
@@ -98,7 +100,26 @@ UavcanGnssBridge::init()
 		return res;
 	}

-	_pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax);
+
+	// UAVCAN_PUB_RTCM
+	int32_t uavcan_pub_rtcm = 0;
+	param_get(param_find("UAVCAN_PUB_RTCM"), &uavcan_pub_rtcm);
+
+	if (uavcan_pub_rtcm == 1) {
+		_publish_rtcm_stream = true;
+		_pub_rtcm_stream.setPriority(uavcan::TransferPriority::NumericallyMax);
+		_rtcm_stream_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm stream pub");
+	}
+
+	// UAVCAN_PUB_MBD
+	int32_t uavcan_pub_mbd = 0;
+	param_get(param_find("UAVCAN_PUB_MBD"), &uavcan_pub_mbd);
+
+	if (uavcan_pub_mbd == 1) {
+		_publish_moving_baseline_data = true;
+		_pub_moving_baseline_data.setPriority(uavcan::TransferPriority::NumericallyMax);
+		_moving_baseline_data_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream pub");
+	}

 	return res;
 }
@@ -282,11 +303,11 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	if (!msg.ecef_position_velocity.empty()) {
 		heading = msg.ecef_position_velocity[0].velocity_xyz[0];

-		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
+		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[1])) {
 			heading_offset = msg.ecef_position_velocity[0].velocity_xyz[1];
 		}

-		if (!isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
+		if (!std::isnan(msg.ecef_position_velocity[0].velocity_xyz[2])) {
 			heading_accuracy = msg.ecef_position_velocity[0].velocity_xyz[2];
 		}
 	}
@@ -446,41 +467,37 @@ void UavcanGnssBridge::update()
 // to work.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
-	bool updated = false;
-
 	// Limit maximum number of GPS injections to 6 since usually
 	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
 	// Looking at 6 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	const size_t max_num_injections = 6;
+	static constexpr size_t MAX_NUM_INJECTIONS = 6;
+
 	size_t num_injections = 0;
+	gps_inject_data_s gps_inject_data;

-	do {
-		num_injections++;
-		updated = _orb_inject_data_sub.updated();
-
-		if (updated) {
-			gps_inject_data_s msg;
-
-			if (_orb_inject_data_sub.copy(&msg)) {
-
-				/* Write the message to the gps device. Note that the message could be fragmented.
-				 * But as we don't write anywhere else to the device during operation, we don't
-				 * need to assemble the message first.
-				 */
-				injectData(msg.data, msg.len);
-			}
+	while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) {
+		// Write the message to the gps device. Note that the message could be fragmented.
+		// But as we don't write anywhere else to the device during operation, we don't
+		// need to assemble the message first.
+		if (_publish_rtcm_stream) {
+			PublishRTCMStream(gps_inject_data.data, gps_inject_data.len);
 		}
-	} while (updated && num_injections < max_num_injections);
+
+		if (_publish_moving_baseline_data) {
+			PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
+		}
+
+		num_injections++;
+	}
 }

-bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len)
+bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)
 {
-	using ardupilot::gnss::MovingBaselineData;
+	using uavcan::equipment::gnss::RTCMStream;

-	perf_count(_rtcm_perf);
-
-	MovingBaselineData msg;
+	RTCMStream msg;
+	msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;

 	const size_t capacity = msg.data.capacity();
 	size_t written = 0;
@@ -498,7 +515,36 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l
 			written += 1;
 		}

-		result = _pub_rtcm.broadcast(msg) >= 0;
+		result = _pub_rtcm_stream.broadcast(msg) >= 0;
+		perf_count(_rtcm_stream_pub_perf);
+		msg.data = {};
+	}
+
+	return result;
+}
+
+bool UavcanGnssBridge::PublishMovingBaselineData(const uint8_t *data, size_t data_len)
+{
+	ardupilot::gnss::MovingBaselineData msg;
+
+	const size_t capacity = msg.data.capacity();
+	size_t written = 0;
+	bool result = true;
+
+	while (result && written < data_len) {
+		size_t chunk_size = data_len - written;
+
+		if (chunk_size > capacity) {
+			chunk_size = capacity;
+		}
+
+		for (size_t i = 0; i < chunk_size; ++i) {
+			msg.data.push_back(data[written]);
+			written += 1;
+		}
+
+		result = _pub_moving_baseline_data.broadcast(msg) >= 0;
+		perf_count(_moving_baseline_data_pub_perf);
 		msg.data.clear();
 	}

@@ -508,5 +554,6 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l
 void UavcanGnssBridge::print_status() const
 {
 	UavcanSensorBridgeBase::print_status();
-	perf_print_counter(_rtcm_perf);
+	perf_print_counter(_rtcm_stream_pub_perf);
+	perf_print_counter(_moving_baseline_data_pub_perf);
 }
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2022 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
@@ -54,6 +54,7 @@
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 #include <ardupilot/gnss/MovingBaselineData.hpp>
+#include <uavcan/equipment/gnss/RTCMStream.hpp>

 #include <lib/perf/perf_counter.h>

@@ -61,8 +62,6 @@

 class UavcanGnssBridge : public UavcanSensorBridgeBase
 {
-	static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10;
-
 public:
 	static const char *const NAME;

@@ -92,7 +91,8 @@ private:
 			  const float heading_accuracy);

 	void handleInjectDataTopic();
-	bool injectData(const uint8_t *data, size_t data_len);
+	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
+	bool PublishMovingBaselineData(const uint8_t *data, size_t data_len);

 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
@@ -115,17 +115,23 @@ private:
 	uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
-	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_rtcm;
+
+	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
+	uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _pub_rtcm_stream;

 	uint64_t	_last_gnss_auxiliary_timestamp{0};
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};

-	uORB::Subscription			_orb_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)};

 	bool _system_clock_set{false};  ///< Have we set the system clock at least once from GNSS data?

 	bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg

-	perf_counter_t _rtcm_perf;
+	bool _publish_rtcm_stream{false};
+	bool _publish_moving_baseline_data{false};
+
+	perf_counter_t _rtcm_stream_pub_perf{nullptr};
+	perf_counter_t _moving_baseline_data_pub_perf{nullptr};
 };
@@ -36,7 +36,9 @@
 */

 #pragma once
-#if defined(UAVCAN_KINETIS_NUTTX)
+#if defined(UAVCAN_SOCKETCAN_NUTTX)
+#  include <uavcan_nuttx/uavcan_nuttx.hpp>
+#elif defined(UAVCAN_KINETIS_NUTTX)
 #  include <uavcan_kinetis/uavcan_kinetis.hpp>
 #elif defined(UAVCAN_STM32_NUTTX)
 #  include <uavcan_stm32/uavcan_stm32.hpp>
@@ -0,0 +1,18 @@
+Libuavcan platform driver for NuttX SocketCAN
+================================================
+
+This document describes the Libuavcan v0 driver for NuttX SocketCAN.
+The libuavcan driver for NuttX is a C++11 library that implements a fully functional platform interface
+for libuavcan and also adds a few convenient wrappers.
+It's built on the following Linux components:
+
+* [SocketCAN](http://en.wikipedia.org/wiki/SocketCAN) -
+A generic CAN bus stack for Linux.
+
+## Usage
+
+Documentation for each feature is provided in the Doxygen comments in the header files.
+
+NuttX applications that use libuavcan need to link the following libraries:
+
+* libuavcan
@@ -0,0 +1,16 @@
+include_directories(
+    ./include
+    )
+
+add_compile_options(-Wno-unused-variable)
+add_library(uavcan_socketcan_driver STATIC
+    src/socketcan.cpp
+    src/thread.cpp
+    )
+
+add_dependencies(uavcan_socketcan_driver uavcan)
+
+install(DIRECTORY include/uavcan_nuttx DESTINATION include)
+install(TARGETS uavcan_socketcan_driver DESTINATION lib)
+
+# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :)
--- a/Show More
+++ b/Show More