RC Loss failsafe param docs

vtol_takeoff: reset reposition triplet before handing over to loiter mode
Signed-off-by: RomanBapst <bapstroman@gmail.com>
2026-06-12 05:00:05 +08:00 · 2022-07-26 20:11:09 -07:00 · 2022-07-25 14:48:18 +02:00 · 2022-07-25 11:48:36 +02:00 · 2022-07-25 11:48:36 +02:00 · 2022-07-25 08:19:00 +02:00
175 changed files with 7984 additions and 3271 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,
@@ -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)
@@ -120,3 +120,11 @@ Additional information about supported hardware can be found in [PX4 user Guide
 ## 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

@@ -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

@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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.
@@ -58,6 +58,18 @@ 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

+# 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
@@ -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
@@ -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
@@ -41,7 +41,7 @@
 * @reboot_required true
 * @group Cyphal
 */
-PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
+PARAM_DEFINE_INT32(CYPHAL_ENABLE, 1);

 /**
 * Cyphal Node ID.
@@ -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
@@ -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 :)
@@ -0,0 +1,232 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <cassert>
+#include <time.h>
+#include <cstdint>
+
+#include <unistd.h>
+#include <sys/time.h>
+#include <sys/types.h>
+
+#include <uavcan/driver/system_clock.hpp>
+
+namespace uavcan_socketcan
+{
+/**
+ * Different adjustment modes can be used for time synchronization
+ */
+enum class ClockAdjustmentMode {
+	SystemWide,      ///< Adjust the clock globally for the whole system; requires root privileges
+	PerDriverPrivate ///< Adjust the clock only for the current driver instance
+};
+
+/**
+ * Linux system clock driver.
+ * Requires librt.
+ */
+class SystemClock : public uavcan::ISystemClock
+{
+	uavcan::UtcDuration private_adj_;
+	uavcan::UtcDuration gradual_adj_limit_;
+	const ClockAdjustmentMode adj_mode_;
+	std::uint64_t step_adj_cnt_;
+	std::uint64_t gradual_adj_cnt_;
+
+	static constexpr std::int64_t Int1e6   = 1000000;
+	static constexpr std::uint64_t UInt1e6 = 1000000;
+
+	bool performStepAdjustment(const uavcan::UtcDuration adjustment)
+	{
+		step_adj_cnt_++;
+		const std::int64_t usec = adjustment.toUSec();
+		timeval tv;
+
+		if (gettimeofday(&tv, NULL) != 0) {
+			return false;
+		}
+
+		tv.tv_sec  += usec / Int1e6;
+		tv.tv_usec += usec % Int1e6;
+		return settimeofday(&tv, nullptr) == 0;
+	}
+
+#ifdef CONFIG_CLOCK_TIMEKEEPING
+	bool performGradualAdjustment(const uavcan::UtcDuration adjustment)
+	{
+		gradual_adj_cnt_++;
+		const std::int64_t usec = adjustment.toUSec();
+		timeval tv;
+		tv.tv_sec  = usec / Int1e6;
+		tv.tv_usec = usec % Int1e6;
+		return adjtime(&tv, nullptr) == 0;
+	}
+#endif
+
+public:
+	/**
+	 * By default, the clock adjustment mode will be selected automatically - global if root, private otherwise.
+	 */
+	explicit SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode())
+		: gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000))
+		, adj_mode_(adj_mode)
+		, step_adj_cnt_(0)
+		, gradual_adj_cnt_(0)
+	{ }
+
+	/**
+	 * Returns monotonic timestamp from librt.
+	 * @throws uavcan_nuttx::Exception.
+	 */
+	uavcan::MonotonicTime getMonotonic() const override
+	{
+		timespec ts;
+
+		if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) {
+			//throw Exception("Failed to get monotonic time");
+		}
+
+		return uavcan::MonotonicTime::fromUSec(std::uint64_t(ts.tv_sec) * UInt1e6 + ts.tv_nsec / 1000);
+	}
+
+	/**
+	 * Returns wall time from gettimeofday().
+	 */
+	uavcan::UtcTime getUtc() const override
+	{
+		timeval tv;
+
+		if (gettimeofday(&tv, NULL) != 0) {
+			//throw Exception("Failed to get UTC time");
+		}
+
+		uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * UInt1e6 + tv.tv_usec);
+
+		if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) {
+			utc += private_adj_;
+		}
+
+		return utc;
+	}
+
+	/**
+	 * Adjusts the wall clock.
+	 * Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode.
+	 * Clock adjustment mode can be set only once via constructor.
+	 *
+	 * If the system wide adjustment mode is selected, two ways for performing adjustment exist:
+	 *  - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit.
+	 *  - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit.
+	 * The gradual adjustment limit can be configured at any time via the setter method.
+	 *
+	 */
+	void adjustUtc(const uavcan::UtcDuration adjustment) override
+	{
+		if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) {
+			private_adj_ += adjustment;
+
+		} else {
+			assert(private_adj_.isZero());
+			assert(!gradual_adj_limit_.isNegative());
+
+#ifdef CONFIG_CLOCK_TIMEKEEPING
+
+			if (adjustment.getAbs() < gradual_adj_limit_) {
+				performGradualAdjustment(adjustment);
+
+			} else
+#endif
+			{
+				performStepAdjustment(adjustment);
+			}
+		}
+	}
+
+	/**
+	 * Sets the maximum phase error to use adjtime().
+	 * If the phase error exceeds this value, settimeofday() will be used instead.
+	 */
+	void setGradualAdjustmentLimit(uavcan::UtcDuration limit)
+	{
+		if (limit.isNegative()) {
+			limit = uavcan::UtcDuration();
+		}
+
+		gradual_adj_limit_ = limit;
+	}
+
+	uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; }
+
+	ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; }
+
+	/**
+	 * This is only applicable if the selected clock adjustment mode is private.
+	 * In system wide mode this method will always return zero duration.
+	 */
+	uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; }
+
+	/**
+	 * Statistics that allows to evaluate clock sync preformance.
+	 */
+	std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; }
+	std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; }
+	std::uint64_t getAdjustmentCount() const
+	{
+		return getStepAdjustmentCount() + getGradualAdjustmentCount();
+	}
+
+	/**
+	 * This static method decides what is the optimal clock sync adjustment mode for the current configuration.
+	 * It selects system wide mode if the application is running as root; otherwise it prefers
+	 * the private adjustment mode because the system wide mode requires root privileges.
+	 */
+	static ClockAdjustmentMode detectPreferredClockAdjustmentMode()
+	{
+		const bool godmode = 0; // geteuid() == 0;
+		return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate;
+	}
+
+	static SystemClock &instance()
+	{
+		static SystemClock self;
+		return self;
+	}
+
+
+};
+
+
+}
@@ -0,0 +1,222 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <uavcan_nuttx/thread.hpp>
+#include <uavcan_nuttx/clock.hpp>
+#include <uavcan/driver/can.hpp>
+
+#include <sys/time.h>
+#include <sys/socket.h>
+
+#include <nuttx/can.h>
+#include <netpacket/can.h>
+
+namespace uavcan_socketcan
+{
+
+class CanIface : public uavcan::ICanIface
+	, uavcan::Noncopyable
+{
+	int               _fd{-1};
+	bool              _can_fd{false};
+
+	//// Send msg structure
+	struct iovec       _send_iov {};
+	struct canfd_frame _send_frame {};
+	struct msghdr      _send_msg {};
+	struct cmsghdr     *_send_cmsg {};
+	struct timeval     *_send_tv {};  /* TX deadline timestamp */
+	uint8_t            _send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {};
+
+	//// Receive msg structure
+	struct iovec       _recv_iov {};
+	struct canfd_frame _recv_frame {};
+	struct msghdr      _recv_msg {};
+	struct cmsghdr     *_recv_cmsg {};
+	uint8_t            _recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {};
+
+	SystemClock clock;
+
+public:
+	uavcan::uint32_t socketInit(const char *can_iface_name);
+
+	uavcan::int16_t send(const uavcan::CanFrame &frame,
+			     uavcan::MonotonicTime tx_deadline,
+			     uavcan::CanIOFlags flags) override;
+
+	uavcan::int16_t receive(uavcan::CanFrame &out_frame,
+				uavcan::MonotonicTime &out_ts_monotonic,
+				uavcan::UtcTime &out_ts_utc,
+				uavcan::CanIOFlags &out_flags) override;
+
+	uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs,
+					 uavcan::uint16_t num_configs) override;
+
+	uavcan::uint64_t getErrorCount() const override;
+
+	uavcan::uint16_t getNumFilters() const override;
+
+	int getFD();
+};
+
+/**
+ * This class implements CAN driver interface for libuavcan.
+ * No configuration needed other than CAN baudrate.
+ */
+class CanDriver
+	: public uavcan::ICanDriver
+	, uavcan::Noncopyable
+{
+	BusEvent update_event_;
+	CanIface if_[UAVCAN_SOCKETCAN_NUM_IFACES];
+	SystemClock clock;
+	struct pollfd pfds[UAVCAN_SOCKETCAN_NUM_IFACES];
+
+public:
+	CanDriver() : update_event_(*this)
+	{}
+
+	uavcan::int32_t initIface(uint32_t index, const char *name)
+	{
+		if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) {
+			return -1;
+		}
+
+		return if_[index].socketInit(name);
+	}
+
+	/**
+	 * Attempts to detect bit rate of the CAN bus.
+	 * This function may block for up to X seconds, where X is the number of bit rates to try.
+	 * This function is NOT guaranteed to reset the CAN controller upon return.
+	 * @return On success: detected bit rate, in bits per second.
+	 *         On failure: zero.
+	 */
+	static uavcan::uint32_t detectBitRate(void (*idle_callback)() = nullptr);
+
+	/**
+	 * Returns negative value if the requested baudrate can't be used.
+	 * Returns zero if OK.
+	 */
+	int init(uavcan::uint32_t bitrate);
+
+	/**
+	 * Returns the number of times the RX queue was overrun.
+	 */
+	uavcan::uint32_t getRxQueueOverflowCount() const;
+
+	/**
+	 * Whether the controller is currently in bus off state.
+	 * Note that the driver recovers the CAN controller from the bus off state automatically!
+	 * Therefore, this method serves only monitoring purposes and is not necessary to use.
+	 */
+	bool isInBusOffState() const;
+
+	uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
+			       const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
+			       uavcan::MonotonicTime blocking_deadline) override;
+
+	uavcan::ICanIface *getIface(uavcan::uint8_t iface_index) override;
+
+	uavcan::uint8_t getNumIfaces() const override;
+
+	BusEvent &updateEvent() { return update_event_; }
+};
+
+
+template <unsigned RxQueueCapacity = 128>
+class CanInitHelper
+{
+	//CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity];
+
+public:
+	enum { BitRateAutoDetect = 0 };
+
+	CanDriver driver;
+
+	CanInitHelper(uint32_t unused = 0x7) :
+		driver()
+	{
+	}
+
+	/**
+	 * This overload simply configures the provided bitrate.
+	 * Auto bit rate detection will not be performed.
+	 * Bitrate value must be positive.
+	 * @return  Negative value on error; non-negative on success. Refer to constants Err*.
+	 */
+	int init(uavcan::uint32_t bitrate)
+	{
+		driver.initIface(0, "can0");
+#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
+		driver.initIface(1, "can1");
+#endif
+		return driver.init(bitrate);
+	}
+
+	/**
+	 * This function can either initialize the driver at a fixed bit rate, or it can perform
+	 * automatic bit rate detection. For theory please refer to the CiA application note #801.
+	 *
+	 * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
+	 *                          The callable entity will be invoked without arguments.
+	 *                          @ref getRecommendedListeningDelay().
+	 *
+	 * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+	 *                          If auto detection was used, the function will update the argument
+	 *                          with established bit rate. In case of an error the value will be undefined.
+	 *
+	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
+	 */
+	template <typename DelayCallable>
+	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = 1000000)
+	{
+		if (inout_bitrate > 0) {
+			return driver.init(inout_bitrate);
+
+		}
+	}
+
+	/**
+	 * Use this value for listening delay during automatic bit rate detection.
+	 */
+	static uavcan::MonotonicDuration getRecommendedListeningDelay()
+	{
+		return uavcan::MonotonicDuration::fromMSec(1050);
+	}
+};
+
+}
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <nuttx/config.h>
+#include <nuttx/fs/fs.h>
+#include <poll.h>
+#include <errno.h>
+#include <cstdio>
+#include <ctime>
+#include <cstring>
+
+#include <uavcan/uavcan.hpp>
+
+namespace uavcan_socketcan
+{
+
+class CanDriver;
+
+
+/**
+ * All bus events are reported as POLLIN.
+ */
+class BusEvent : uavcan::Noncopyable
+{
+	using SignalCallbackHandler = void(*)();
+
+	SignalCallbackHandler signal_cb_{nullptr};
+	sem_t sem_;
+public:
+
+	BusEvent(CanDriver &can_driver);
+	~BusEvent();
+
+	void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
+
+	bool wait(uavcan::MonotonicDuration duration);
+
+	void signalFromInterrupt();
+};
+
+class Mutex
+{
+	pthread_mutex_t mutex_;
+
+public:
+	Mutex()
+	{
+		init();
+	}
+
+	int init()
+	{
+		return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
+	}
+
+	int deinit()
+	{
+		return pthread_mutex_destroy(&mutex_);
+	}
+
+	void lock()
+	{
+		(void)pthread_mutex_lock(&mutex_);
+	}
+
+	void unlock()
+	{
+		(void)pthread_mutex_unlock(&mutex_);
+	}
+};
+
+
+class MutexLocker
+{
+	Mutex &mutex_;
+
+public:
+	MutexLocker(Mutex &mutex)
+		: mutex_(mutex)
+	{
+		mutex_.lock();
+	}
+	~MutexLocker()
+	{
+		mutex_.unlock();
+	}
+};
+
+}
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <uavcan/uavcan.hpp>
+
+#include <uavcan_nuttx/thread.hpp>
+#include <uavcan_nuttx/clock.hpp>
+#include <uavcan_nuttx/socketcan.hpp>
@@ -0,0 +1,344 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ *
+ *
+ */
+
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <uavcan_nuttx/socketcan.hpp>
+#include <uavcan_nuttx/clock.hpp>
+#include <uavcan/util/templates.hpp>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <string.h>
+
+#include <nuttx/can.h>
+#include <netpacket/can.h>
+
+#define MODULE_NAME "UAVCAN_SOCKETCAN"
+
+#include <px4_platform_common/log.h>
+
+namespace uavcan_socketcan
+{
+namespace
+{
+
+struct BitTimingSettings {
+	std::uint32_t canclkdiv;
+	std::uint32_t canbtr;
+
+	bool isValid() const { return canbtr != 0; }
+};
+
+} // namespace
+
+uavcan::uint32_t CanIface::socketInit(const char *can_iface_name)
+{
+
+	struct sockaddr_can addr;
+	struct ifreq ifr;
+
+	//FIXME Change this when we update to DroneCAN with CAN FD support
+	bool can_fd = 0;
+
+	_can_fd = can_fd;
+
+	/* open socket */
+	if ((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+		PX4_ERR("socket");
+		return -1;
+	}
+
+	strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1);
+	ifr.ifr_name[IFNAMSIZ - 1] = '\0';
+	ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
+
+	if (!ifr.ifr_ifindex) {
+		PX4_ERR("if_nametoindex");
+		return -1;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.can_family = AF_CAN;
+	addr.can_ifindex = ifr.ifr_ifindex;
+
+	const int on = 1;
+	/* RX Timestamping */
+
+	if (setsockopt(_fd, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) {
+		PX4_ERR("SO_TIMESTAMP is disabled");
+		return -1;
+	}
+
+	/* NuttX Feature: Enable TX deadline when sending CAN frames
+	 * When a deadline occurs the driver will remove the CAN frame
+	 */
+
+	if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) {
+		PX4_ERR("CAN_RAW_TX_DEADLINE is disabled");
+		return -1;
+	}
+
+	if (can_fd) {
+		if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) {
+			PX4_ERR("no CAN FD support");
+			return -1;
+		}
+	}
+
+	if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		PX4_ERR("bind");
+		return -1;
+	}
+
+	// Setup TX msg
+	_send_iov.iov_base = &_send_frame;
+
+	if (_can_fd) {
+		_send_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		_send_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(&_send_control, 0x00, sizeof(_send_control));
+
+	_send_msg.msg_iov    = &_send_iov;
+	_send_msg.msg_iovlen = 1;
+	_send_msg.msg_control = &_send_control;
+	_send_msg.msg_controllen = sizeof(_send_control);
+
+	_send_cmsg = CMSG_FIRSTHDR(&_send_msg);
+	_send_cmsg->cmsg_level = SOL_CAN_RAW;
+	_send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE;
+	_send_cmsg->cmsg_len = sizeof(struct timeval);
+	_send_tv = (struct timeval *)CMSG_DATA(_send_cmsg);
+
+	// Setup RX msg
+	_recv_iov.iov_base = &_recv_frame;
+
+	if (can_fd) {
+		_recv_iov.iov_len = sizeof(struct canfd_frame);
+
+	} else {
+		_recv_iov.iov_len = sizeof(struct can_frame);
+	}
+
+	memset(_recv_control, 0x00, sizeof(_recv_control));
+
+	_recv_msg.msg_iov = &_recv_iov;
+	_recv_msg.msg_iovlen = 1;
+	_recv_msg.msg_control = &_recv_control;
+	_recv_msg.msg_controllen = sizeof(_recv_control);
+	_recv_cmsg = CMSG_FIRSTHDR(&_recv_msg);
+
+	return 0;
+}
+
+uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
+			       uavcan::CanIOFlags flags)
+{
+	int res = -1;
+
+	/* Copy CanardFrame to can_frame/canfd_frame */
+	if (_can_fd) {
+		_send_frame.can_id = frame.id | CAN_EFF_FLAG;
+		_send_frame.len = frame.dlc;
+		memcpy(&_send_frame.data, frame.data, frame.dlc);
+
+	} else {
+		struct can_frame *net_frame = (struct can_frame *)&_send_frame;
+		net_frame->can_id = frame.id | CAN_EFF_FLAG;
+		net_frame->can_dlc = frame.dlc;
+		memcpy(&net_frame->data, frame.data, frame.dlc);
+	}
+
+	/* Set CAN_RAW_TX_DEADLINE timestamp  */
+	_send_tv->tv_usec = tx_deadline.toUSec() % 1000000ULL;
+	_send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 1000000ULL;
+
+	res = sendmsg(_fd, &_send_msg, 0);
+
+	if (res > 0) {
+		return 1;
+
+	} else {
+		return res;
+	}
+}
+
+uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
+				  uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags)
+{
+	int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);
+
+	if (result < 0) {
+		return result;
+	}
+
+	/* Copy SocketCAN frame to CanardFrame */
+
+	if (_can_fd) {
+		struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame;
+		out_frame.id = recv_frame->can_id;
+		out_frame.dlc = recv_frame->len;
+		memcpy(out_frame.data, &recv_frame->data, recv_frame->len);
+
+	} else {
+		struct can_frame *recv_frame = (struct can_frame *)&_recv_frame;
+		out_frame.id = recv_frame->can_id;
+		out_frame.dlc = recv_frame->can_dlc;
+		memcpy(out_frame.data, &recv_frame->data, recv_frame->can_dlc);
+	}
+
+	/* Read SO_TIMESTAMP value */
+
+	if (_recv_cmsg->cmsg_level == SOL_SOCKET && _recv_cmsg->cmsg_type == SO_TIMESTAMP) {
+		struct timeval *tv = (struct timeval *)CMSG_DATA(_recv_cmsg);
+		out_ts_monotonic = uavcan::MonotonicTime::fromUSec(tv->tv_sec * 1000000ULL + tv->tv_usec);
+	}
+
+	return result;
+}
+
+
+uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs,
+		uavcan::uint16_t num_configs)
+{
+	//FIXME
+	return 0;
+}
+
+uavcan::uint64_t CanIface::getErrorCount() const
+{
+	//FIXME query SocketCAN network stack
+	return 0;
+}
+
+uavcan::uint16_t CanIface::getNumFilters() const
+{
+	//FIXME
+	return 0;
+}
+
+int CanIface::getFD()
+{
+	return _fd;
+}
+
+uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
+{
+	//FIXME
+	return 1;
+}
+
+int CanDriver::init(uavcan::uint32_t bitrate)
+{
+	pfds[0].fd     = if_[0].getFD();
+	pfds[0].events = POLLIN | POLLOUT;
+#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
+	pfds[1].fd     = if_[1].getFD();
+	pfds[1].events = POLLIN | POLLOUT;
+#endif
+
+	/*
+	 * TODO add filter configuration ioctl
+	 */
+
+	return 0;
+}
+
+uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const
+{
+	//FIXME query SocketCAN network stack
+	return 0;
+}
+
+bool CanDriver::isInBusOffState() const
+{
+	//FIXME no interface available yet, maybe make a NuttX ioctl
+	return false;
+}
+
+uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks,
+				  const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
+				  uavcan::MonotonicTime blocking_deadline)
+{
+	std::int64_t timeout_usec = (blocking_deadline - clock.getMonotonic()).toUSec();
+
+	if (timeout_usec < 0) {
+		timeout_usec = 0;
+	}
+
+	inout_masks.read = 0;
+	//FIXME NuttX SocketCAN implement POLLOUT
+	inout_masks.write = 0x3;
+
+	if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) {
+		for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
+			if (pfds[i].revents & POLLIN) {
+				inout_masks.read |= 1U << i;
+			}
+
+			if (pfds[i].revents & POLLOUT) {
+				inout_masks.write |= 1U << i;
+			}
+		}
+	}
+
+	return 0;           // Return value doesn't matter as long as it is non-negative
+}
+
+
+uavcan::ICanIface *CanDriver::getIface(uavcan::uint8_t iface_index)
+{
+	if (iface_index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) {
+		return nullptr;
+	}
+
+	return &if_[iface_index];
+}
+
+uavcan::uint8_t CanDriver::getNumIfaces() const
+{
+	return UAVCAN_SOCKETCAN_NUM_IFACES;
+}
+
+}
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * 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 <uavcan_nuttx/thread.hpp>
+#include <uavcan_nuttx/socketcan.hpp>
+
+namespace uavcan_socketcan
+{
+
+BusEvent::BusEvent(CanDriver &can_driver)
+{
+	sem_init(&sem_, 0, 0);
+	sem_setprotocol(&sem_, SEM_PRIO_NONE);
+}
+
+BusEvent::~BusEvent()
+{
+	sem_destroy(&sem_);
+}
+
+bool BusEvent::wait(uavcan::MonotonicDuration duration)
+{
+	if (duration.isPositive()) {
+		timespec abstime;
+
+		if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
+			const unsigned billion = 1000 * 1000 * 1000;
+			uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
+			abstime.tv_sec += nsecs / billion;
+			nsecs -= (nsecs / billion) * billion;
+			abstime.tv_nsec = nsecs;
+
+			int ret;
+
+			while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
+
+			if (ret == -1) { // timed out or error
+				return false;
+			}
+
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void BusEvent::signalFromInterrupt()
+{
+	if (sem_.semcount <= 0) {
+		(void)sem_post(&sem_);
+	}
+
+	if (signal_cb_) {
+		signal_cb_();
+	}
+}
+
+}
@@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
 set(UAVCAN_PLATFORM "generic")

 if(CONFIG_ARCH_CHIP)
-	if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
+	if(${CONFIG_NET_CAN} MATCHES "y")
+		set(UAVCAN_DRIVER "socketcan")
+		set(UAVCAN_TIMER 1)
+	elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
 		set(UAVCAN_DRIVER "kinetis")
 		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
@@ -61,6 +61,8 @@ using namespace time_literals;
 namespace uavcannode
 {

+
+
 /**
 * @file uavcan_main.cpp
 *
@@ -92,7 +94,8 @@ boot_app_shared_section app_descriptor_t AppDescriptor = {

 UavcanNode *UavcanNode::_instance;

-UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
+UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver,
+		       uavcan::ISystemClock &system_clock) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
 	_node(can_driver, system_clock, _pool_allocator),
 	_time_sync_slave(_node),
@@ -102,6 +105,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 {
 	int res = pthread_mutex_init(&_node_mutex, nullptr);

+	_can = can_init;
+	_bitrate = bitrate;
+
 	if (res < 0) {
 		std::abort();
 	}
@@ -158,39 +164,27 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 		return -1;
 	}

-	/*
-	 * CAN driver init
-	 * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
-	 * shipped with libuavcan does not support deinitialization.
-	 */
-	static CanInitHelper *can = nullptr;
+	static CanInitHelper *_can = nullptr;

-	if (can == nullptr) {
+	if (_can == nullptr) {

-		can = new CanInitHelper();
+		_can = new CanInitHelper();

-		if (can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
+		if (_can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
 			PX4_ERR("Out of memory");
 			return -1;
 		}
-
-		const int can_init_res = can->init(bitrate);
-
-		if (can_init_res < 0) {
-			PX4_ERR("CAN driver init failed %i", can_init_res);
-			return can_init_res;
-		}
 	}

 	// Node init
-	_instance = new UavcanNode(can->driver, UAVCAN_DRIVER::SystemClock::instance());
+	_instance = new UavcanNode(_can, bitrate, _can->driver, UAVCAN_DRIVER::SystemClock::instance());

 	if (_instance == nullptr) {
 		PX4_ERR("Out of memory");
 		return -1;
 	}

-	const int node_init_res = _instance->init(node_id, can->driver.updateEvent());
+	const int node_init_res = _instance->init(node_id, _can->driver.updateEvent());

 	if (node_init_res < 0) {
 		delete _instance;
@@ -334,45 +328,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events

 	bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);

-	int rv = _node.start();
-
-	if (rv < 0) {
-		return rv;
-	}
-
-	// If the node_id was not supplied by the bootloader do Dynamic Node ID allocation
-
-	if (node_id == 0) {
-
-		uavcan::DynamicNodeIDClient client(_node);
-
-		int client_start_res = client.start(_node.getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
-						    node_id);
-
-		if (client_start_res < 0) {
-			PX4_ERR("Failed to start the dynamic node ID client");
-			return client_start_res;
-		}
-
-		watchdog_pet(); // If allocation takes too long reboot
-
-		/*
-		 * Waiting for the client to obtain a node ID.
-		 * This may take a few seconds.
-		 */
-
-		while (!client.isAllocationComplete()) {
-			const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200));    // Spin duration doesn't matter
-
-			if (res < 0) {
-				PX4_ERR("Transient failure: %d", res);
-			}
-		}
-
-		_node.setNodeID(client.getAllocatedNodeID());
-	}
-
-	return rv;
+	return 1;
 }

 // Restart handler
@@ -397,6 +353,51 @@ void UavcanNode::Run()
 	watchdog_pet();

 	if (!_initialized) {
+
+
+		const int can_init_res = _can->init(_bitrate);
+
+		if (can_init_res < 0) {
+			PX4_ERR("CAN driver init failed %i", can_init_res);
+		}
+
+		int rv = _node.start();
+
+		if (rv < 0) {
+			PX4_ERR("Failed to start the node");
+		}
+
+		// If the node_id was not supplied by the bootloader do Dynamic Node ID allocation
+
+		if (_node.getNodeID() == 0) {
+
+			uavcan::DynamicNodeIDClient client(_node);
+
+			int client_start_res = client.start(_node.getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
+							    _node.getNodeID());
+
+			if (client_start_res < 0) {
+				PX4_ERR("Failed to start the dynamic node ID client");
+			}
+
+			watchdog_pet(); // If allocation takes too long reboot
+
+			/*
+			 * Waiting for the client to obtain a node ID.
+			 * This may take a few seconds.
+			 */
+
+			while (!client.isAllocationComplete()) {
+				const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200));    // Spin duration doesn't matter
+
+				if (res < 0) {
+					PX4_ERR("Transient failure: %d", res);
+				}
+			}
+
+			_node.setNodeID(client.getAllocatedNodeID());
+		}
+
 		up_time = hrt_absolute_time();
 		get_node().setRestartRequestHandler(&restart_request_handler);
 		_param_server.start(&_param_manager);
@@ -120,7 +120,8 @@ public:
 	typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 	typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;

-	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+	UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver,
+		   uavcan::ISystemClock &system_clock);

 	virtual		~UavcanNode();

@@ -139,6 +140,9 @@ public:

 	/* The bit rate that can be passed back to the bootloader */
 	int32_t active_bitrate{0};
+	uint32_t _bitrate;
+
+	CanInitHelper *_can;

 private:
 	void Run() override;
@@ -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>
@@ -34,6 +34,7 @@
 #pragma once

 #include <stdint.h>
+#include <stddef.h>

 namespace px4
 {
@@ -60,14 +60,19 @@
 #endif


+#define system_exit exit
+#define system_clock_gettime clock_gettime
+#define system_clock_settime clock_settime
+#define system_pthread_cond_timedwait pthread_cond_timedwait
+#define system_usleep usleep
+#define system_sleep sleep

-
+#ifndef PX4_DISABLE_GCC_POISON

 /* exit() is used on NuttX to exit a task. However on Posix, it will exit the
 * whole application, so we prevent its use there. There are cases where it
 * still needs to be used, thus we remap system_exit to exit.
 */
-#define system_exit exit
 #if !defined(__PX4_NUTTX)
 #include <stdlib.h>
 #ifdef __cplusplus
@@ -83,8 +88,6 @@
 /* For SITL lockstep we fake the clock, sleeping, and timedwaits
 * Therefore, we prefix these syscalls with system_. */
 #include <time.h>
-#define system_clock_gettime clock_gettime
-#define system_clock_settime clock_settime
 /* We can't poison clock_settime/clock_gettime because they are
 * used in DriverFramework. */

@@ -93,20 +96,17 @@
 // We can't include this for NuttX otherwise we get conflicts for read/write
 // symbols in cannode.
 #endif // !defined(__PX4_NUTTX)
-#define system_pthread_cond_timedwait pthread_cond_timedwait
 /* We can't poison pthread_cond_timedwait because it seems to be used in the
 * <string> include. */


 /* We don't poison usleep and sleep because it is used in dependencies
- * like uavcan and DriverFramework. */
+ * like uavcan. */
 #if !defined(__PX4_NUTTX)
 #include <unistd.h>
 // We can't include this for NuttX otherwise we get conflicts for read/write
 // symbols in cannode.
 #endif // !defined(__PX4_NUTTX)
-#define system_usleep usleep
-#define system_sleep sleep


 /* On NuttX we call clearenv() so we cannot use getenv() and others (see
@@ -123,3 +123,5 @@
 * need to get changed. */
 #pragma GCC poison getenv setenv putenv
 #endif // defined(__PX4_NUTTX)
+
+#endif // PX4_DISABLE_GCC_POISON
@@ -48,7 +48,6 @@ add_subdirectory(field_sensor_bias_estimator)
 add_subdirectory(geo)
 add_subdirectory(hysteresis)
 add_subdirectory(l1)
-add_subdirectory(landing_slope)
 add_subdirectory(led)
 add_subdirectory(matrix)
 add_subdirectory(mathlib)
@@ -40,6 +40,8 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>

+#include <stdlib.h>
+
 const cdev::px4_file_operations_t cdev::CDev::fops = {};

 pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
@@ -52,12 +52,10 @@

 #define CBRK_BUZZER_KEY         782097
 #define CBRK_SUPPLY_CHK_KEY	894281
-#define CBRK_RATE_CTRL_KEY	140253
 #define CBRK_IO_SAFETY_KEY	22027
 #define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_USB_CHK_KEY	197848
-#define CBRK_VELPOSERR_KEY	201607
 #define CBRK_VTOLARMING_KEY	159753

 #include <stdint.h>
@@ -56,21 +56,6 @@
 */
 PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);

-/**
- * Circuit breaker for rate controller output
- *
- * Setting this parameter to 140253 will disable the rate
- * controller uORB publication.
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @reboot_required true
- * @min 0
- * @max 140253
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
-
 /**
 * Circuit breaker for IO safety
 *
@@ -149,20 +134,6 @@ PARAM_DEFINE_INT32(CBRK_BUZZER, 0);
 */
 PARAM_DEFINE_INT32(CBRK_USB_CHK, 197848);

-/**
- * Circuit breaker for position error check
- *
- * Setting this parameter to 201607 will disable the position and velocity
- * accuracy checks in the commander.
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @min 0
- * @max 201607
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_VELPOSERR, 0);
-
 /**
 * Circuit breaker for arming in fixed-wing mode check
 *
@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file landingslope.cpp
- *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#include "Landingslope.hpp"
-
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
-
-void
-Landingslope::update(float landing_slope_angle_rad_new,
-		     float flare_relative_alt_new,
-		     float motor_lim_relative_alt_new,
-		     float H1_virt_new)
-{
-	_landing_slope_angle_rad = landing_slope_angle_rad_new;
-	_flare_relative_alt = flare_relative_alt_new;
-	_motor_lim_relative_alt = motor_lim_relative_alt_new;
-	_H1_virt = H1_virt_new;
-
-	calculateSlopeValues();
-}
-
-void
-Landingslope::calculateSlopeValues()
-{
-	_H0 =  _flare_relative_alt + _H1_virt;
-	_d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad);
-	_flare_constant = (_H0 * _d1) / _flare_relative_alt;
-	_flare_length = -logf(_H1_virt / _H0) * _flare_constant;
-	_horizontal_slope_displacement = (_flare_length - _d1);
-}
-
-float
-Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
-{
-	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement,
-			_landing_slope_angle_rad);
-}
-
-float
-Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-		float bearing_airplane_currwp)
-{
-	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
-	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
-		return getLandingSlopeRelativeAltitude(wp_landing_distance);
-
-	}
-
-	return 0.0f;
-}
-
-float
-Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-		float bearing_airplane_currwp)
-{
-	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
-	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
-		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;
-
-	}
-
-	return 0.0f;
-}
-
-/**
- *
- * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
- */
-float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
-		float landing_slope_angle_rad)
-{
-	// flare_relative_alt is negative
-	return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad);
-}
-
-/**
- *
- * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
- */
-float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
-		float horizontal_slope_displacement, float landing_slope_angle_rad)
-{
-	return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement,
-					       landing_slope_angle_rad) + wp_landing_altitude;
-}
-
-/**
- *
- * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
- */
-float Landingslope::getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
-		float horizontal_slope_displacement, float landing_slope_angle_rad)
-{
-	return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
-}
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file landingslope.h
- *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#ifndef LANDINGSLOPE_H_
-#define LANDINGSLOPE_H_
-
-#include <math.h>
-
-class Landingslope
-{
-private:
-	/* see Documentation/fw_landing.png for a plot of the landing slope */
-	float _landing_slope_angle_rad{0.0f};		/**< phi in the plot */
-	float _flare_relative_alt{0.0f};		/**< h_flare,rel in the plot */
-	float _motor_lim_relative_alt{0.0f};
-	float _H1_virt{0.0f};				/**< H1 in the plot */
-	float _H0{0.0f};				/**< h_flare,rel + H1 in the plot */
-	float _d1{0.0f};				/**< d1 in the plot */
-	float _flare_constant{0.0f};
-	float _flare_length{0.0f};			/**< d1 + delta d in the plot */
-	float _horizontal_slope_displacement{0.0f};	/**< delta d in the plot */
-
-	void calculateSlopeValues();
-
-public:
-	Landingslope() = default;
-	~Landingslope() = default;
-
-	/**
-	 *
-	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	float getLandingSlopeRelativeAltitude(float wp_landing_distance);
-
-	/**
-	 *
-	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 * Performs check if aircraft is in front of waypoint to avoid climbout
-	 */
-	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-			float bearing_airplane_currwp);
-
-	/**
-	 *
-	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
-			float landing_slope_angle_rad);
-
-	/**
-	 *
-	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
-			float horizontal_slope_displacement, float landing_slope_angle_rad);
-
-	/**
-	 *
-	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
-	 */
-	static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
-					       float horizontal_slope_displacement, float landing_slope_angle_rad);
-
-	float getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-						float bearing_airplane_currwp);
-
-	void update(float landing_slope_angle_rad_new,
-		    float flare_relative_alt_new,
-		    float motor_lim_relative_alt_new,
-		    float H1_virt_new);
-
-
-	float landing_slope_angle_rad() { return _landing_slope_angle_rad; }
-	float flare_relative_alt() { return _flare_relative_alt; }
-	float motor_lim_relative_alt() { return _motor_lim_relative_alt; }
-	float flare_length() { return _flare_length; }
-	float horizontal_slope_displacement() { return _horizontal_slope_displacement; }
-
-};
-
-
-#endif /* LANDINGSLOPE_H_ */
@@ -31,8 +31,14 @@
 #
 ############################################################################

+if(CONFIG_ARCH_CHIP_STM32H7)
+	set(FLASHFS_SOURCE_FILE flashfs32.c)
+else()
+	set(FLASHFS_SOURCE_FILE flashfs.c)
+endif()
+
 add_library(flashparams
-	flashfs.c
+	${FLASHFS_SOURCE_FILE}
 	flashparams.cpp
 )

@@ -28,8 +28,8 @@ table {
 }
 </style>

-"""               
-                  )    
+"""
+                  )

        for group in groups:
            result += '## %s\n\n' % group.GetName()
@@ -37,7 +37,7 @@ table {
 """<table>
 <colgroup><col style="width: 23%"><col style="width: 46%"><col style="width: 11%"><col style="width: 11%"><col style="width: 9%"></colgroup>
 <thead>
-   <tr><th>Name</th><th>Description</th><th>Min > Max (Incr.)</th><th>Default</th><th>Units</th></tr>
+   <tr><th>Name</th><th>Description</th><th>[Min, Max] (Incr.)</th><th>Default</th><th>Units</th></tr>
 </thead>
 <tbody>
 """
@@ -68,7 +68,7 @@ table {
                        min_val='?'
                    if not max_val:
                        max_val='?'
-                    max_min_combined+='%s > %s ' % (min_val, max_val)
+                    max_min_combined+='[%s, %s] ' % (min_val, max_val)
                if increment:
                    max_min_combined+='(%s)' % increment

@@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f;	///< max value of _dt allowed before a fil
 * which is used by the airspeed complimentary filter.
 */
 void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward,
-		bool altitude_lock, bool in_air, float altitude, float vz)
+		bool altitude_lock, float altitude, float vz)
 {
 	// calculate the time lapsed since the last update
 	uint64_t now = hrt_absolute_time();
@@ -69,7 +69,7 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		reset_altitude = true;
 	}

-	if (!altitude_lock || !in_air) {
+	if (!altitude_lock) {
 		reset_altitude = true;
 	}

@@ -80,8 +80,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 	_state_update_timestamp = now;
 	_EAS = equivalent_airspeed;

-	_in_air = in_air;
-
 	// Set the velocity and position state to the the INS data
 	_vert_vel_state = -vz;
 	_vert_pos_state = altitude;
@@ -97,11 +95,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		_tas_rate_raw = 0.0f;
 		_tas_rate_filtered = 0.0f;
 	}
-
-	if (!_in_air) {
-		_states_initialized = false;
-	}
-
 }

 void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS)
@@ -124,8 +117,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 		_EAS = equivalent_airspeed;
 	}

-	// If first time through or not flying, reset airspeed states
-	if (_speed_update_timestamp == 0 || !_in_air) {
+	// If first time through reset airspeed states
+	if (_speed_update_timestamp == 0) {
 		_tas_rate_state = 0.0f;
 		_tas_state = (_EAS * EAS2TAS);
 	}
@@ -136,18 +129,21 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	_tas_innov = (_EAS * EAS2TAS) - _tas_state;
 	float tas_rate_state_input = _tas_innov * _tas_estimate_freq * _tas_estimate_freq;

-	// limit integrator input to prevent windup
-	if (_tas_state < 3.1f) {
-		tas_rate_state_input = max(tas_rate_state_input, 0.0f);
-	}
-
 	// Update TAS state
 	_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
 	float tas_state_input = _tas_rate_state + _tas_rate_raw + _tas_innov * _tas_estimate_freq * 1.4142f;
-	_tas_state = _tas_state + tas_state_input * dt;
+	const float new_tas_state = _tas_state + tas_state_input * dt;
+
+	if (new_tas_state < 0.0f) {
+		// clip TAS at zero, back calculate rate
+		tas_state_input = -_tas_state / dt;
+		_tas_rate_state = tas_state_input - _tas_rate_raw - _tas_innov * _tas_estimate_freq * 1.4142f;
+		_tas_state = 0.0f;
+
+	} else {
+		_tas_state = new_tas_state;
+	}

-	// Limit the TAS state to a minimum of 3 m/s
-	_tas_state = max(_tas_state, 3.0f);
 	_speed_update_timestamp = now;

 }
@@ -156,23 +152,32 @@ void TECS::_update_speed_setpoint()
 {
 	// Set the TAS demand to the minimum value if an underspeed or
 	// or a uncontrolled descent condition exists to maximise climb rate
-	if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
+	if (_uncommanded_descent_recovery) {
 		_TAS_setpoint = _TAS_min;
+
+	} else if (_percent_undersped > FLT_EPSILON) {
+		// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
+		// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
+		// from this line back into this method each time).
+		// TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to
+		// avoid the fear of side effects in simple operations like this.
+		_TAS_setpoint = _TAS_min * _percent_undersped + (1.0f - _percent_undersped) * _TAS_setpoint;
 	}

 	_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);

 	// Calculate limits for the demanded rate of change of speed based on physical performance limits
 	// with a 50% margin to allow the total energy controller to correct for errors.
-	float velRateMax = 0.5f * _STE_rate_max / _tas_state;
-	float velRateMin = 0.5f * _STE_rate_min / _tas_state;
+	const float max_tas_rate_sp = 0.5f * _STE_rate_max / math::max(_tas_state, FLT_EPSILON);
+	const float min_tas_rate_sp = 0.5f * _STE_rate_min / math::max(_tas_state, FLT_EPSILON);

 	_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);

 	// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
 	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
 	if (airspeed_sensor_enabled()) {
-		_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
+		_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, min_tas_rate_sp,
+					       max_tas_rate_sp);

 	} else {
 		_TAS_rate_setpoint = 0.0f;
@@ -207,18 +212,24 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
 void TECS::_detect_underspeed()
 {
 	if (!_detect_underspeed_enabled) {
-		_underspeed_detected = false;
+		_percent_undersped = 0.0f;
 		return;
 	}

-	if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
-	    || ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) {
+	// this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed
+	// to vary in before ramping in underspeed mitigation
+	const float tas_error_bound = kTASErrorPercentage * _equivalent_airspeed_trim;

-		_underspeed_detected = true;
+	// this is the soft boundary where underspeed mitigation is ramped in
+	// NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general
+	// need to be the same
+	const float tas_underspeed_soft_bound = kTASErrorPercentage * _equivalent_airspeed_trim;

-	} else {
-		_underspeed_detected = false;
-	}
+	const float tas_fully_undersped = math::max(_TAS_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
+	const float tas_starting_to_underspeed = math::max(_TAS_min - tas_error_bound, tas_fully_undersped);
+
+	_percent_undersped = 1.0f - math::constrain((_tas_state - tas_fully_undersped) /
+			     math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
 }

 void TECS::_update_energy_estimates()
@@ -259,90 +270,84 @@ void TECS::_update_throttle_setpoint()
 	_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint);
 	_STE_rate_error = _STE_rate_error_filter.getState();

-	float throttle_setpoint;
+	// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
+	// Assume induced drag scales linearly with normal load factor.
+	// The additional normal load factor is given by (1/cos(bank angle) - 1)
+	_STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);

-	// Calculate the throttle demand
-	if (_underspeed_detected) {
-		// always use full throttle to recover from an underspeed condition
-		throttle_setpoint = _throttle_setpoint_max;
+	_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+
+	// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
+	// as the starting point. Assume:
+	// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
+	// Specific total energy rate = 0 at cruise throttle
+	// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
+	float throttle_predicted = 0.0f;
+
+	if (_STE_rate_setpoint >= 0) {
+		// throttle is between trim and maximum
+		throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - _throttle_trim);

 	} else {
-		// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
-		// Assume induced drag scales linearly with normal load factor.
-		// The additional normal load factor is given by (1/cos(bank angle) - 1)
-		_STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);
+		// throttle is between trim and minimum
+		throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - _throttle_trim);

-		_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+	}

-		// Calculate a predicted throttle from the demanded rate of change of energy, using the trim throttle
-		// as the starting point. Assume:
-		// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
-		// Specific total energy rate = 0 at trim throttle
-		// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
-		float throttle_predicted = 0.0f;
+	// Calculate gain scaler from specific energy rate error to throttle
+	const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);

-		if (_STE_rate_setpoint >= 0) {
-			// throttle is between trim and maximum
-			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max -
-					     _throttle_trim);
+	// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
+	float throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
+	throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);

-		} else {
-			// throttle is between trim and minimum
-			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min -
-					     _throttle_trim);
+	// Integral handling
+	if (airspeed_sensor_enabled()) {
+		if (_integrator_gain_throttle > 0.0f) {
+			float integ_state_max = _throttle_setpoint_max - throttle_setpoint;
+			float integ_state_min = _throttle_setpoint_min - throttle_setpoint;

-		}
+			// underspeed conditions zero out integration
+			float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt *
+						     STE_rate_to_throttle * (1.0f - _percent_undersped);

-		// Calculate gain scaler from specific energy rate error to throttle
-		const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
+			// only allow integrator propagation into direction which unsaturates throttle
+			if (_throttle_integ_state > integ_state_max) {
+				throttle_integ_input = math::min(0.f, throttle_integ_input);

-		// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
-		throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
-		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
-
-		if (airspeed_sensor_enabled()) {
-			if (_integrator_gain_throttle > 0.0f) {
-				float integ_state_max = _throttle_setpoint_max - throttle_setpoint;
-				float integ_state_min = _throttle_setpoint_min - throttle_setpoint;
-
-				float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt *
-							     STE_rate_to_throttle;
-
-				// only allow integrator propagation into direction which unsaturates throttle
-				if (_throttle_integ_state > integ_state_max) {
-					throttle_integ_input = math::min(0.f, throttle_integ_input);
-
-				} else if (_throttle_integ_state < integ_state_min) {
-					throttle_integ_input = math::max(0.f, throttle_integ_input);
-				}
-
-				// Calculate a throttle demand from the integrated total energy rate error
-				// This will be added to the total throttle demand to compensate for steady state errors
-				_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
-
-				if (_climbout_mode_active) {
-					// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
-					// at end of climbout when we transition to closed loop throttle control
-					_throttle_integ_state = integ_state_max;
-				}
-
-			} else {
-				_throttle_integ_state = 0.0f;
+			} else if (_throttle_integ_state < integ_state_min) {
+				throttle_integ_input = math::max(0.f, throttle_integ_input);
 			}

-		}
+			// Calculate a throttle demand from the integrated total energy rate error
+			// This will be added to the total throttle demand to compensate for steady state errors
+			_throttle_integ_state = _throttle_integ_state + throttle_integ_input;

-		if (airspeed_sensor_enabled()) {
-			// Add the integrator feedback during closed loop operation with an airspeed sensor
-			throttle_setpoint += _throttle_integ_state;
+			if (_climbout_mode_active) {
+				// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
+				// at end of climbout when we transition to closed loop throttle control
+				_throttle_integ_state = integ_state_max;
+			}

 		} else {
-			// when flying without an airspeed sensor, use the predicted throttle only
-			throttle_setpoint = throttle_predicted;
-
+			_throttle_integ_state = 0.0f;
 		}
+
 	}

+	if (airspeed_sensor_enabled()) {
+		// Add the integrator feedback during closed loop operation with an airspeed sensor
+		throttle_setpoint += _throttle_integ_state;
+
+	} else {
+		// when flying without an airspeed sensor, use the predicted throttle only
+		throttle_setpoint = throttle_predicted;
+
+	}
+
+	// ramp in max throttle setting with underspeediness value
+	throttle_setpoint = _percent_undersped * _throttle_setpoint_max + (1.0f - _percent_undersped) * throttle_setpoint;
+
 	// Rate limit the throttle demand
 	if (fabsf(_throttle_slewrate) > 0.01f) {
 		const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
@@ -364,13 +369,15 @@ void TECS::_detect_uncommanded_descent()
 	// Calculate rate of change of total specific energy
 	const float STE_rate = _SPE_rate + _SKE_rate;

+	const bool underspeed_detected = _percent_undersped > FLT_EPSILON;
+
 	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
-	const bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f)
+	const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (_STE_error > 200.0f)
 				&& (STE_rate < 0.0f)
 				&& (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f);

 	// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
-	const bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
+	const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (_STE_error < 0.0f));

 	if (enter_mode) {
 		_uncommanded_descent_recovery = true;
@@ -442,9 +449,10 @@ void TECS::_update_pitch_setpoint()
 	float pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);

 	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
-	const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
-	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - ptchRateIncr,
-					 _last_pitch_setpoint + ptchRateIncr);
+	// NOTE: at zero airspeed, the pitch increment is unbounded
+	const float pitch_increment = _dt * _vert_accel_limit / _tas_state;
+	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - pitch_increment,
+					 _last_pitch_setpoint + pitch_increment);
 }

 void TECS::_updateTrajectoryGenerationConstraints()
@@ -493,32 +501,29 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout,
 			      float EAS2TAS)
 {
-	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
+	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_states_initialized) {
 		// On first time through or when not using TECS of if there has been a large time slip,
 		// states must be reset to allow filters to a clean start
 		_vert_vel_state = 0.0f;
 		_vert_pos_state = baro_altitude;
 		_tas_rate_state = 0.0f;
 		_tas_state = _EAS * EAS2TAS;
-		_throttle_integ_state =  0.0f;
-		_pitch_integ_state = 0.0f;
-		_last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);;
+		_last_throttle_setpoint = throttle_trim;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
 		_TAS_setpoint_adj = _TAS_setpoint_last;
-		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
 		_STE_rate_error = 0.0f;
 		_hgt_setpoint = baro_altitude;

+		resetIntegrals();
+
 		if (_dt > DT_MAX || _dt < DT_MIN) {
 			_dt = DT_DEFAULT;
 		}

-		_alt_control_traj_generator.reset(0, 0, baro_altitude);
-		_velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude);
-
+		resetTrajectoryGenerators(baro_altitude);

 	} else if (_climbout_mode_active) {
 		// During climbout use the lower pitch angle limit specified by the
@@ -534,8 +539,6 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit

 		_hgt_setpoint = baro_altitude;

-		// disable speed and decent error condition checks
-		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
 	}

@@ -553,10 +556,10 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit
 void TECS::_update_STE_rate_lim()
 {
 	// Calculate the specific total energy upper rate limits from the max throttle climb rate
-	_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
+	_STE_rate_max = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;

 	// Calculate the specific total energy lower rate limits from the min throttle sink rate
-	_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
+	_STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
 }

 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
@@ -580,11 +583,6 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Initialize selected states and variables as required
 	_initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas);

-	// Don't run TECS control algorithms when not in flight
-	if (!_in_air) {
-		return;
-	}
-
 	_updateTrajectoryGenerationConstraints();

 	// Update the true airspeed state estimate
@@ -619,7 +617,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_pitch_update_timestamp = now;

 	// Set TECS mode for next frame
-	if (_underspeed_detected) {
+	if (_percent_undersped > FLT_EPSILON) {
 		_tecs_mode = ECL_TECS_MODE_UNDERSPEED;

 	} else if (_uncommanded_descent_recovery) {
@@ -638,17 +636,21 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 void TECS::_update_speed_height_weights()
 {
 	// Calculate the weight applied to control of specific kinetic energy error
-	_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
+	float pitch_speed_weight = constrain(_pitch_speed_weight, 0.0f, 2.0f);

-	if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
-		_SKE_weighting = 2.0f;
+	if (_climbout_mode_active && airspeed_sensor_enabled()) {
+		pitch_speed_weight = 2.0f;
+
+	} else if (_percent_undersped > FLT_EPSILON && airspeed_sensor_enabled()) {
+		pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight;

 	} else if (!airspeed_sensor_enabled()) {
-		_SKE_weighting = 0.0f;
+		pitch_speed_weight = 0.0f;
+
 	}

 	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
 	// loop time constant and therefore can lead to a destabilization of that control loop
-	_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
-	_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
+	_SPE_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
+	_SKE_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
 }
--- a/Show More
+++ b/Show More