vehicle_status: Remove duplicate failure detector status

.clang-tidy

@@ -1,17 +1,11 @@
 ---
 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,
@@ -24,7 +18,6 @@ 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,
@@ -44,7 +37,8 @@ Checks:          '*,
                   -cppcoreguidelines-pro-type-union-access,
                   -cppcoreguidelines-pro-type-vararg,
                   -cppcoreguidelines-special-member-functions,
-                  -fuchsia-*,
+                  -fuchsia-default-arguments,
+                  -fuchsia-overloaded-operator,
                   -google-build-using-namespace,
                   -google-explicit-constructor,
                   -google-global-names-in-headers,
@@ -68,7 +62,6 @@ Checks:          '*,
                   -hicpp-use-equals-delete,
                   -hicpp-use-override,
                   -hicpp-vararg,
-                  -llvmlibc-*,
                   -llvm-header-guard,
                   -llvm-include-order,
                   -llvm-namespace-comment,
@@ -91,7 +84,6 @@ 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,

CMakeLists.txt

@@ -175,12 +175,6 @@ 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)

README.md

@@ -120,11 +120,3 @@ 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>

ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane

@@ -10,6 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
+param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 12
 

ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal

@@ -7,9 +7,12 @@
 
 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
 
@@ -35,6 +38,7 @@ 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

ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric

@@ -7,9 +7,12 @@
 
 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
 
@@ -35,6 +38,7 @@ 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

ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod

@@ -10,6 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
+param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 15
 

ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo

@@ -7,9 +7,12 @@
 
 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
 
@@ -34,6 +37,7 @@ 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
 

ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer

@@ -10,6 +10,7 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
+param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 12
 

ROMFS/px4fmu_common/init.d-posix/airframes/17002_tf-g2

@@ -1,55 +0,0 @@
-#!/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

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -78,7 +78,6 @@ 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

ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil

@@ -29,6 +29,9 @@ 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

ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard

@@ -1,10 +1,22 @@
 #!/bin/sh
 #
-# @name Generic Standard VTOL
+# @name Generic Quadplane 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
@@ -12,21 +24,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 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_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_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -1
-param set-default CA_ROTOR3_PY 1
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
 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
@@ -36,5 +48,16 @@ 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

ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor

@@ -1,44 +0,0 @@
-#!/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

ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter

@@ -1,10 +1,17 @@
 #!/bin/sh
 #
-# @name Generic VTOL Tailsitter
+# @name Generic Tailsitter
 #
-# @type VTOL Tailsitter
+# @type VTOL Duo 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
@@ -12,13 +19,18 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
-param set-default SYS_CTRL_ALLOC 1
+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 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
@@ -27,6 +39,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
 
-param set-default MAV_TYPE 19
-param set-default VT_TYPE 0
-param set-default VT_ELEV_MC_LOCK 0
+set MIXER vtol_tailsitter_duo
+
+set PWM_OUT 1234

ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane

@@ -1,16 +1,28 @@
 #!/bin/sh
 #
-# @name Generic Standard Plane
+# @name 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
@@ -22,3 +34,11 @@ 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.

ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing

@@ -5,20 +5,19 @@
 # @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
 
-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
+set MIXER fw_generic_wing

ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5

@@ -30,6 +30,9 @@ 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

ROMFS/px4fmu_common/init.d/airframes/3033_wingwing

@@ -32,6 +32,9 @@ 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

ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha

@@ -27,6 +27,9 @@ 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

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -124,7 +124,6 @@ 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

boards/mro/pixracerpro/default.px4board

@@ -13,7 +13,6 @@ 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
@@ -39,7 +38,6 @@ 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
@@ -48,7 +46,6 @@ 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
@@ -64,6 +61,7 @@ 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
@@ -72,9 +70,10 @@ 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_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y

boards/mro/pixracerpro/init/rc.board_sensors

@@ -8,14 +8,8 @@ board_adc start
 icm20602 -s -b 1 -R 8 start
 
 # Internal SPI bus BMI088 accel & gyro
-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
+bmi088 -A -s -b 5 -R 8 start
+bmi088 -G -s -b 5 -R 8 start
 
 # Internal ICM-20948 (with magnetometer)
 icm20948 -s -b 1 -R 8 -M start

boards/mro/pixracerpro/src/spi.cpp

@@ -47,8 +47,6 @@ 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}),

boards/nxp/ucans32k146/cyphal.px4board

@@ -1,4 +0,0 @@
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_CLIENT=y
-CONFIG_CYPHAL_APP_DESCRIPTOR=y
-CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y

boards/nxp/ucans32k146/default.px4board

@@ -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_BOARD_UAVCAN_INTERFACES=1
-CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_CLIENT=y
+CONFIG_CYPHAL_APP_DESCRIPTOR=y
+CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=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,6 +23,5 @@ 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

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -6,9 +6,4 @@
 pwm_out mode_pwm1 start
 
 ifup can0
-
-# Start Cyphal when enabled
-if param compare -s CYPHAL_ENABLE 1
-then
-    cyphal start
-fi
+cyphal start

boards/nxp/ucans32k146/nuttx-config/nsh/defconfig

@@ -112,6 +112,6 @@ CONFIG_SYMTAB_ORDEREDBYNAME=y
 CONFIG_SYSTEM_I2CTOOL=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_SPITOOL=y
-CONFIG_USERMAIN_STACKSIZE=2176
 CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_USERMAIN_STACKSIZE=2176
 CONFIG_WATCHDOG=y

boards/px4/fmu-v5x/init/rc.board_sensors

@@ -12,7 +12,6 @@ fi
 if param compare -s ADC_ADS1115_EN 1
 then
 	ads1115 start -X
-	board_adc start -n
 else
 	board_adc start
 fi

boards/px4/fmu-v5x/rtps.px4board

@@ -4,7 +4,6 @@ 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

boards/px4/sitl/src/board_shutdown.cpp

@@ -40,7 +40,6 @@
 #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)

msg/gps_inject_data.msg

@@ -2,10 +2,8 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 
-uint16 len                       # length of data
+uint8 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

msg/position_controller_landing_status.msg

@@ -1,16 +1,9 @@
-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
+uint64 timestamp		# time since system start (microseconds)
 
-# 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 horizontal_slope_displacement
 
-# 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
+float32 slope_angle_rad
+
+float32 flare_length
+
+bool abort_landing				# true if landing should be aborted

msg/sensor_gps.msg

@@ -46,7 +46,4 @@ 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

msg/templates/uorb_microcdr/microRTPS_client.cpp.em

@@ -253,14 +253,10 @@ void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, ui
 
 	while (!_should_exit_task) {
 @[if recv_topics]@
-
-		read = transport_node->read();
-		if (read > 0) {
+		while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
 			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) {

msg/templates/urtps/microRTPS_agent.cpp.em

@@ -364,14 +364,10 @@ int main(int argc, char **argv)
 		if (!receiving) { start = std::chrono::steady_clock::now(); }
 
 		// Publish messages received from UART
-		length = transport_node->read();
-		if (length > 0) {
-			total_read += length;
-		}
-
-		while (transport_node->parse(&topic_ID, data_buffer, BUFFER_SIZE)) {
+		if (0 < (length = transport_node->read(&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();
 		}

msg/templates/urtps/microRTPS_transport.cpp

@@ -113,8 +113,14 @@ uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len)
 	return crc;
 }
 
-ssize_t Transport_node::read()
+ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer_len)
 {
+	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) {
@@ -137,141 +143,85 @@ ssize_t Transport_node::read()
 
 	_rx_buff_pos += len;
 
-	return len;
-}
+	// We read some
+	size_t header_size = sizeof(struct Header);
 
-#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(" ");
-	}
-
-	printf("\n_rx_buff_pos: %" PRIu32 "\n", _rx_buff_pos);
-}
-
-#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.
+	// but not enough
 	if (_rx_buff_pos < header_size) {
-		return false;
+		return 0;
 	}
 
-	// Try to find the start of a message using the magic start sequence of '>>>'.
-	int32_t msg_start_pos = -1;
+	uint32_t msg_start_pos = 0;
 
-	for (uint32_t i = 0; i <= _rx_buff_pos; ++i) {
-		if ('>' == _rx_buffer[i] && memcmp(_rx_buffer + i, ">>>", 3) == 0) {
-			msg_start_pos = i;
+	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) {
 			break;
 		}
 	}
 
-	auto DropDataBeforeIndex = [&](uint32_t index) {
-		if (index == 0) {
-			return;
-		}
-
-		if (index > _rx_buff_pos) {
-			index = _rx_buff_pos;
-
+	// Start not found
+	if (msg_start_pos > (_rx_buff_pos - header_size)) {
 #ifndef PX4_DEBUG
 
-			if (_debug) { printf("\033[0;31m[ micrortps_transport ]\t clamped index from %" PRIu32 " to %" PRIu32 "\033[0m\n", index, _rx_buff_pos); }
+		if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t                                (↓↓ %" PRIu32 ")\033[0m\n", msg_start_pos); }
 
 #else
 
-			if (_debug) { PX4_ERR("ERROR: clamped index from %" PRIu32 " to %" PRIu32 "\n", index, _rx_buff_pos); }
+		if (_debug) { PX4_DEBUG("                               (↓↓ %" PRIu32 ")", msg_start_pos); }
 
 #endif /* PX4_DEBUG */
-		}
 
-		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;
+		// 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;
 	}
 
-	// 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;
-	}
+	// [>,>,>,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;
 
 	// 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) {
-		DropDataBeforeIndex(3);
-		return false;
+		// 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;
 	}
 
-	// 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 + header_size, payload_len);
+	uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + msg_start_pos + header_size, payload_len);
 
 	if (read_crc != calc_crc) {
 #ifndef PX4_DEBUG
@@ -284,16 +234,26 @@ bool Transport_node::parse(
 
 #endif /* PX4_DEBUG */
 
-		DropDataBeforeIndex(3);
-		return false;
+		// 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);
 	}
 
-	// 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;
+	return len;
 }
 
 size_t Transport_node::get_header_length()

msg/templates/urtps/microRTPS_transport.h

@@ -56,9 +56,7 @@ public:
 
 	virtual int init() {return 0;}
 	virtual uint8_t close() {return 0;}
-
-	ssize_t read();
-	bool parse(uint8_t *topic_id, char out_buffer[], size_t buffer_len);
+	ssize_t read(uint8_t *topic_id, char out_buffer[], size_t buffer_len);
 
 	/**
 	 * write a buffer
@@ -78,10 +76,6 @@ 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;

msg/vehicle_local_position.msg

@@ -64,8 +64,6 @@ 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)

msg/vehicle_rates_setpoint.msg

@@ -1,9 +1,8 @@
 uint64 timestamp	# time since system start (microseconds)
 
-# 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
+float32 roll		# body angular rates in NED frame
+float32 pitch		# body angular rates in NED frame
+float32 yaw		# body angular rates in NED frame
 
 # 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.

msg/vehicle_status.msg

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

msg/vehicle_status_flags.msg

@@ -29,6 +29,7 @@ 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

platforms/nuttx/src/px4/common/board_crashdump.c

@@ -38,10 +38,10 @@
  * and hardfault log support
  */
 
-#include <board_config.h>
-
 #ifdef CONFIG_BOARD_CRASHDUMP
 
+#include <board_config.h>
+
 #include <stdio.h>
 #include <stdbool.h>
 #include <string.h>

platforms/nuttx/src/px4/common/cdc_acm_check.cpp

@@ -31,8 +31,6 @@
  *
  ****************************************************************************/
 
-#include <board_config.h>
-
 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
 #include <arch/board/board.h>

platforms/nuttx/src/px4/common/nuttx_random.c

@@ -41,4 +41,6 @@ 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

platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt

@@ -40,4 +40,3 @@ 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)

platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp

@@ -39,7 +39,6 @@
  */
 
 #include <px4_platform_common/px4_config.h>
-#include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <hardware/s32k1xx_rcm.h>
@@ -54,26 +53,6 @@ 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
  *

platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h

@@ -32,8 +32,6 @@
  ****************************************************************************/
 #pragma once
 
-#include <board_config.h>
-
 #if defined(CONFIG_SPI)
 
 #include "../../../stm32_common/include/px4_arch/spi_hw_description.h"

platforms/posix/cmake/sitl_target.cmake

@@ -394,7 +394,6 @@ if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
 		rascal
 		rascal-electric
 		tf-g1
-		tf-g2
 		tf-r1
 	)
 	set(all_posix_vmd_make_targets)

platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h

@@ -39,7 +39,6 @@
 #include <memory>
 #include <atomic>
 #include <pthread.h>
-#include <unistd.h>
 
 #include "lockstep_components.h"
 

src/drivers/adc/board_adc/ADC.cpp

@@ -39,9 +39,8 @@
 #include <nuttx/ioexpander/gpio.h>
 #endif
 
-ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
+ADC::ADC(uint32_t base_address, uint32_t channels) :
 	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)
 {
@@ -117,10 +116,7 @@ void ADC::Run()
 		_samples[i].am_data = sample(_samples[i].am_channel);
 	}
 
-	if (_publish_adc_report) {
-		update_adc_report(now);
-	}
-
+	update_adc_report(now);
 	update_system_power(now);
 }
 
@@ -356,8 +352,7 @@ int ADC::custom_command(int argc, char *argv[])
 
 int ADC::task_spawn(int argc, char *argv[])
 {
-	bool publish_adc_report = !(argc >= 2 && strcmp(argv[1], "-n") == 0);
-	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report);
+	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
 
 	if (instance) {
 		_object.store(instance);
@@ -394,7 +389,6 @@ 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;

src/drivers/adc/board_adc/ADC.hpp

@@ -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, bool publish_adc_report = true);
+	ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS);
 
 	~ADC() override;
 
@@ -102,8 +102,6 @@ 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};
@@ -112,7 +110,6 @@ 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

src/drivers/cyphal/parameters.c

@@ -41,7 +41,7 @@
  * @reboot_required true
  * @group Cyphal
  */
-PARAM_DEFINE_INT32(CYPHAL_ENABLE, 1);
+PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
 
 /**
  * Cyphal Node ID.

src/drivers/drv_sensor.h

@@ -137,8 +137,7 @@
 #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

src/drivers/gps/gps.cpp

@@ -78,11 +78,9 @@
 #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 5_s
+#define RATE_MEASUREMENT_PERIOD 5000000
 
 enum class gps_driver_mode_t {
 	None = 0,
@@ -193,8 +191,6 @@ 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;
 
@@ -515,24 +511,6 @@ 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
@@ -547,6 +525,7 @@ void GPS::handleInjectDataTopic()
 		updated = _orb_inject_data_sub.updated();
 
 		if (updated) {
+			gps_inject_data_s msg;
 
 			if (_orb_inject_data_sub.copy(&msg)) {
 
@@ -559,7 +538,6 @@ void GPS::handleInjectDataTopic()
 					injectData(msg.data, msg.len);
 
 					++_last_rate_rtcm_injection_count;
-					_last_rtcm_injection_time = hrt_absolute_time();
 				}
 			}
 		}
@@ -1165,9 +1143,6 @@ 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.

src/drivers/imu/bosch/bmi085/BMI085.cpp

@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}

src/drivers/imu/bosch/bmi085/BMI085.hpp

@@ -1,81 +0,0 @@
-/****************************************************************************
- *
- *   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
-
-};

src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp

@@ -1,604 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp

@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp

@@ -1,466 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp

@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/Bosch_BMI085_Accelerometer_Registers.hpp

@@ -1,171 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/Bosch_BMI085_Gyroscope_Registers.hpp

@@ -1,141 +0,0 @@
-/****************************************************************************
- *
- *   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

src/drivers/imu/bosch/bmi085/Kconfig

@@ -1,5 +0,0 @@
-menuconfig DRIVERS_IMU_BOSCH_BMI085
-	bool "bosch bmi085"
-	default n
-	---help---
-		Enable support for bosch bmi085

src/drivers/imu/bosch/bmi085/bmi085_main.cpp

@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- *   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;
-}

src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp

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

src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp

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

src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp

@@ -41,7 +41,6 @@
 #pragma once
 
 #include <cstdint>
-#include <cstddef>
 
 namespace InvenSense_ICM20948
 {

src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp

@@ -41,7 +41,6 @@
 #pragma once
 
 #include <cstdint>
-#include <cstddef>
 
 namespace InvenSense_ICM42688P
 {

src/drivers/magnetometer/rm3100/rm3100.cpp

@@ -63,21 +63,13 @@ int RM3100::self_test()
 {
 	bool complete = false;
 
-	// Set the default command mode and enable polling (not continuous mode)
-	uint8_t cmd = (CMM_DEFAULT & ~CONTINUOUS_MODE);
+	uint8_t cmd = (CMM_DEFAULT | POLLING_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);
@@ -86,20 +78,27 @@ int RM3100::self_test()
 		return ret;
 	}
 
-	// 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
-
+	/* 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);
 
@@ -120,16 +119,8 @@ int RM3100::self_test()
 			if (cmd & BIST_STE) {
 				complete = true;
 
-				// 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
+				// If the test passed, disable self-test mode by clearing the STE bit
+				if (cmd & BIST_XYZ_OK) {
 					cmd = 0;
 					ret = _interface->write(ADDR_BIST, &cmd, 1);
 
@@ -138,6 +129,9 @@ int RM3100::self_test()
 					}
 
 					return PX4_OK;
+
+				} else {
+					PX4_ERR("built-in self test failed");
 				}
 			}
 		}
@@ -267,9 +261,6 @@ 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);
 

src/drivers/px4io/px4io.cpp

@@ -531,6 +531,11 @@ 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();
@@ -649,9 +654,6 @@ void PX4IO::Run()
 
 	_mixing_output.updateSubscriptions(true, true);
 
-	// minimal backup scheduling
-	ScheduleDelayed(20_ms);
-
 	perf_end(_cycle_perf);
 }
 

src/drivers/stub_keystore/Kconfig

@@ -1,6 +1,7 @@
 menu "stub_keystore configuration"
 	menuconfig DRIVERS_STUB_KEYSTORE
 	bool "stub_keystore"
+	depends on DRIVERS_SW_CRYPTO
 	default n
 	---help---
 		Enable support for stub_keystore

src/drivers/transponder/sagetech_mxs/module.yaml

@@ -4,4 +4,5 @@ serial_config:
       port_config_param:
         name: MXS_SER_CFG
         group: Transponder
+        default: TEL2
       label: Sagetech MXS Serial Port

src/drivers/uavcan/uavcan_driver.hpp

@@ -36,9 +36,7 @@
  */
 
 #pragma once
-#if defined(UAVCAN_SOCKETCAN_NUTTX)
-#  include <uavcan_nuttx/uavcan_nuttx.hpp>
-#elif defined(UAVCAN_KINETIS_NUTTX)
+#if defined(UAVCAN_KINETIS_NUTTX)
 #  include <uavcan_kinetis/uavcan_kinetis.hpp>
 #elif defined(UAVCAN_STM32_NUTTX)
 #  include <uavcan_stm32/uavcan_stm32.hpp>

src/drivers/uavcan/uavcan_drivers/socketcan/README.md

@@ -1,18 +0,0 @@
-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

src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt

@@ -1,16 +0,0 @@
-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 :)

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp

@@ -1,232 +0,0 @@
-/****************************************************************************
- *
- *   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;
-	}
-
-
-};
-
-
-}

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp

@@ -1,222 +0,0 @@
-/****************************************************************************
- *
- *   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);
-	}
-};
-
-}

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp

@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   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();
-	}
-};
-
-}

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp

@@ -1,41 +0,0 @@
-/****************************************************************************
- *
- *   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>

src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp

@@ -1,344 +0,0 @@
-/*
- * 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;
-}
-
-}

src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp

@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   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_();
-	}
-}
-
-}

src/drivers/uavcannode/CMakeLists.txt

@@ -40,10 +40,7 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
 set(UAVCAN_PLATFORM "generic")
 
 if(CONFIG_ARCH_CHIP)
-	if(${CONFIG_NET_CAN} MATCHES "y")
-		set(UAVCAN_DRIVER "socketcan")
-		set(UAVCAN_TIMER 1)
-	elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
+	if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
 		set(UAVCAN_DRIVER "kinetis")
 		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")

src/drivers/uavcannode/UavcanNode.cpp

@@ -61,8 +61,6 @@ using namespace time_literals;
 namespace uavcannode
 {
 
-
-
 /**
  * @file uavcan_main.cpp
  *
@@ -94,8 +92,7 @@ boot_app_shared_section app_descriptor_t AppDescriptor = {
 
 UavcanNode *UavcanNode::_instance;
 
-UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver,
-		       uavcan::ISystemClock &system_clock) :
+UavcanNode::UavcanNode(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),
@@ -105,9 +102,6 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr
 {
 	int res = pthread_mutex_init(&_node_mutex, nullptr);
 
-	_can = can_init;
-	_bitrate = bitrate;
-
 	if (res < 0) {
 		std::abort();
 	}
@@ -164,27 +158,39 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 		return -1;
 	}
 
-	static CanInitHelper *_can = nullptr;
+	/*
+	 * 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;
 
-	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, bitrate, _can->driver, UAVCAN_DRIVER::SystemClock::instance());
+	_instance = new UavcanNode(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;
@@ -328,7 +334,45 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 
 	bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
 
-	return 1;
+	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;
 }
 
 // Restart handler
@@ -353,51 +397,6 @@ 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);

src/drivers/uavcannode/UavcanNode.hpp

@@ -120,8 +120,7 @@ public:
 	typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
 	typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;
 
-	UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver,
-		   uavcan::ISystemClock &system_clock);
+	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
 
 	virtual		~UavcanNode();
 
@@ -140,9 +139,6 @@ 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;

src/drivers/uavcannode/uavcan_driver.hpp

@@ -36,9 +36,7 @@
  */
 
 #pragma once
-#if defined(UAVCAN_SOCKETCAN_NUTTX)
-#  include <uavcan_nuttx/uavcan_nuttx.hpp>
-#elif defined(UAVCAN_KINETIS_NUTTX)
+#if defined(UAVCAN_KINETIS_NUTTX)
 #  include <uavcan_kinetis/uavcan_kinetis.hpp>
 #elif defined(UAVCAN_STM32_NUTTX)
 #  include <uavcan_stm32/uavcan_stm32.hpp>

src/include/containers/Bitset.hpp

@@ -34,7 +34,6 @@
 #pragma once
 
 #include <stdint.h>
-#include <stddef.h>
 
 namespace px4
 {

src/include/visibility.h

@@ -60,19 +60,14 @@
 #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
@@ -88,6 +83,8 @@
 /* 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. */
 
@@ -96,17 +93,20 @@
 // 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. */
+ * like uavcan and DriverFramework. */
 #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,5 +123,3 @@
  * need to get changed. */
 #pragma GCC poison getenv setenv putenv
 #endif // defined(__PX4_NUTTX)
-
-#endif // PX4_DISABLE_GCC_POISON

src/lib/CMakeLists.txt

@@ -48,6 +48,7 @@ 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)

src/lib/cdev/posix/cdev_platform.cpp

@@ -40,8 +40,6 @@
 #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;

src/lib/circuit_breaker/circuit_breaker.h

@@ -52,10 +52,12 @@
 
 #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>

src/lib/circuit_breaker/circuit_breaker_params.c

@@ -56,6 +56,21 @@
  */
 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
  *
@@ -134,6 +149,20 @@ 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
  *

src/lib/landing_slope/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018 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,24 +31,4 @@
 #
 ############################################################################
 
-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
-	)
+px4_add_library(landing_slope Landingslope.cpp)

src/lib/landing_slope/Landingslope.cpp

@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   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;
+}

src/lib/landing_slope/Landingslope.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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_ */

src/lib/parameters/flashparams/CMakeLists.txt

@@ -31,14 +31,8 @@
 #
 ############################################################################
 
-if(CONFIG_ARCH_CHIP_STM32H7)
-	set(FLASHFS_SOURCE_FILE flashfs32.c)
-else()
-	set(FLASHFS_SOURCE_FILE flashfs.c)
-endif()
-
 add_library(flashparams
-	${FLASHFS_SOURCE_FILE}
+	flashfs.c
 	flashparams.cpp
 )
 

src/lib/parameters/px4params/markdownout.py

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

src/lib/tecs/TECS.cpp

@@ -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, float altitude, float vz)
+		bool altitude_lock, bool in_air, 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) {
+	if (!altitude_lock || !in_air) {
 		reset_altitude = true;
 	}
 
@@ -80,6 +80,8 @@ 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;
@@ -95,6 +97,11 @@ 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)
@@ -117,8 +124,8 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 		_EAS = equivalent_airspeed;
 	}
 
-	// If first time through reset airspeed states
-	if (_speed_update_timestamp == 0) {
+	// If first time through or not flying, reset airspeed states
+	if (_speed_update_timestamp == 0 || !_in_air) {
 		_tas_rate_state = 0.0f;
 		_tas_state = (_EAS * EAS2TAS);
 	}
@@ -129,21 +136,18 @@ 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;
-	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;
-	}
+	_tas_state = _tas_state + tas_state_input * dt;
 
+	// Limit the TAS state to a minimum of 3 m/s
+	_tas_state = max(_tas_state, 3.0f);
 	_speed_update_timestamp = now;
 
 }
@@ -152,32 +156,23 @@ 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) {
+	if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
 		_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.
-	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);
+	float velRateMax = 0.5f * _STE_rate_max / _tas_state;
+	float velRateMin = 0.5f * _STE_rate_min / _tas_state;
 
 	_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, min_tas_rate_sp,
-					       max_tas_rate_sp);
+		_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
 
 	} else {
 		_TAS_rate_setpoint = 0.0f;
@@ -212,24 +207,18 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
 void TECS::_detect_underspeed()
 {
 	if (!_detect_underspeed_enabled) {
-		_percent_undersped = 0.0f;
+		_underspeed_detected = false;
 		return;
 	}
 
-	// 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;
+	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 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;
+		_underspeed_detected = true;
 
-	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);
+	} else {
+		_underspeed_detected = false;
+	}
 }
 
 void TECS::_update_energy_estimates()
@@ -270,84 +259,90 @@ 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();
 
-	// 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);
+	float throttle_setpoint;
 
-	_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);
+	// Calculate the throttle demand
+	if (_underspeed_detected) {
+		// always use full throttle to recover from an underspeed condition
+		throttle_setpoint = _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);
+		// 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);
 
-	}
+		_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
 
-	// Calculate gain scaler from specific energy rate error to throttle
-	const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
+		// 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;
 
-	// 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);
-
-	// 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);
-
-			// 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;
-			}
+		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 {
-			_throttle_integ_state = 0.0f;
+			// throttle is between trim and minimum
+			throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min -
+					     _throttle_trim);
+
 		}
 
+		// Calculate gain scaler from specific energy rate error to throttle
+		const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
+
+		// 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;
+			}
+
+		}
+
+		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;
+
+		}
 	}
 
-	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;
@@ -369,15 +364,13 @@ 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;
@@ -449,10 +442,9 @@ 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
-	// 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);
+	const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
+	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - ptchRateIncr,
+					 _last_pitch_setpoint + ptchRateIncr);
 }
 
 void TECS::_updateTrajectoryGenerationConstraints()
@@ -501,29 +493,32 @@ 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 || !_states_initialized) {
+	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_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;
-		_last_throttle_setpoint = throttle_trim;
+		_throttle_integ_state =  0.0f;
+		_pitch_integ_state = 0.0f;
+		_last_throttle_setpoint = (_in_air ? throttle_trim : 0.0f);;
 		_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;
 		}
 
-		resetTrajectoryGenerators(baro_altitude);
+		_alt_control_traj_generator.reset(0, 0, baro_altitude);
+		_velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude);
+
 
 	} else if (_climbout_mode_active) {
 		// During climbout use the lower pitch angle limit specified by the
@@ -539,6 +534,8 @@ 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;
 	}
 
@@ -556,10 +553,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 = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
+	_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
 
 	// Calculate the specific total energy lower rate limits from the min throttle sink rate
-	_STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
+	_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
 }
 
 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
@@ -583,6 +580,11 @@ 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
@@ -617,7 +619,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 (_percent_undersped > FLT_EPSILON) {
+	if (_underspeed_detected) {
 		_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
 
 	} else if (_uncommanded_descent_recovery) {
@@ -636,21 +638,17 @@ 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
-	float pitch_speed_weight = constrain(_pitch_speed_weight, 0.0f, 2.0f);
+	_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 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;
+	if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
+		_SKE_weighting = 2.0f;
 
 	} else if (!airspeed_sensor_enabled()) {
-		pitch_speed_weight = 0.0f;
-
+		_SKE_weighting = 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 - pitch_speed_weight, 0.f, 1.f);
-	_SKE_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
+	_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
+	_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
 }