preflight check: modify actuator_sp if running

this adds a new flag to ActuatorEffectivenessTiltrotorVTOL:
_preflight_check_running. We set it from ControlAllocator, and if true
we always add collective tilt to the actuator setpoint.

.vscode/cmake-variants.yaml

@@ -131,6 +131,11 @@ CONFIG:
       buildType: MinSizeRel
       settings:
         CONFIG: ark_can-flow_canbootloader
+    ark_can-flow-mr_canbootloader:
+      short: ark_can-flow-mr_canbootloader
+      buildType: MinSizeRel
+      settings:
+        CONFIG: ark_can-flow-mr_canbootloader
     ark_can-gps_default:
       short: ark_can-gps_default
       buildType: MinSizeRel

.vscode/settings.json

@@ -129,5 +129,4 @@
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
         },
-        "ros.distro": "humble"
 }

ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx

@@ -16,10 +16,6 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 
-# disable some checks to allow to fly:
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 # Square quadrotor X PX4 numbering
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1

ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane

@@ -16,12 +16,6 @@ param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
-# disable some checks to allow to fly:
-# - with usb
-param set-default CBRK_USB_CHK 197848
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 param set-default SIH_T_MAX 6
 param set-default SIH_MASS 0.3
 param set-default SIH_IXX 0.00402
@@ -33,18 +27,19 @@ param set-default SIH_KDV 0.2
 param set-default SIH_VEHICLE_TYPE 1 	# sih as fixed wing
 param set-default RWTO_TKOFF 1  # enable takeoff from runway (as opposed to launched)
 
-
 param set-default CA_AIRFRAME 1
-
 param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
 
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
 param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
 param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS1_TYPE 3  # elevator
 param set-default CA_SV_CS2_TRQ_Y 1
-param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
 param set-default PWM_MAIN_FUNC1 201
 param set-default PWM_MAIN_FUNC2 202
 param set-default PWM_MAIN_FUNC3 203

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -28,10 +28,6 @@ param set-default MC_PITCH_P 5
 
 param set-default MAV_TYPE 19
 
-# disable some checks to allow to fly:
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 param set-default SIH_T_MAX 2
 param set-default SIH_Q_MAX 0.0165
 param set-default SIH_MASS 0.2

ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol

@@ -0,0 +1,96 @@
+#!/bin/sh
+#
+# @name SIH Standard VTOL
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Ailerons (single channel)
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+param set-default EKF2_GPS_DELAY 0
+
+param set-default VT_TYPE 2
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR0_PX 0.2
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PX -0.2
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR2_PX 0.2
+param set-default CA_ROTOR2_PY -0.2
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.2
+param set-default CA_ROTOR3_PY 0.2
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_PX -0.3
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 1
+param set-default CA_ROTOR4_AZ 0
+
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3  # elevator
+param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+param set-default FW_AIRSPD_MAX 12
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 201
+param set-default PWM_MAIN_FUNC6 202
+param set-default PWM_MAIN_FUNC7 203
+param set-default PWM_MAIN_FUNC8 105
+
+param set-default MAV_TYPE 22
+
+# set SYS_HITL to 2 to start the SIH and avoid sensors startup
+# param set-default SYS_HITL 2
+
+param set-default SENS_DPRES_OFF 0.001
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.2
+
+# sih as standard vtol
+param set SIH_VEHICLE_TYPE 3

ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart

@@ -0,0 +1,155 @@
+#!/bin/sh
+#
+# @name 6DoF Spacecraft Model
+#
+# @type Freeflyer with 8 thrusters
+#
+# @maintainer Pedro Roque <padr@kth.se>
+#
+
+. ${R}etc/init.d/rc.sc_defaults
+
+param set-default CA_AIRFRAME 15
+param set-default MAV_TYPE 99
+
+param set-default CA_THRUSTER_CNT 12
+param set-default CA_R_REV 0
+
+# param set-default FW_ARSP_MODE 1
+
+# Auto to be provided by Custom Airframe
+param set-default CA_METHOD 0
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
+# Set proper failsafes
+param set-default COM_ACT_FAIL_ACT 0
+param set-default COM_LOW_BAT_ACT 0
+param set-default NAV_DLL_ACT 0
+param set-default GF_ACTION 1
+param set-default NAV_RCL_ACT 1
+param set-default COM_POSCTL_NAVL 2
+
+# Set thrusters
+param set-default CA_THRUSTER0_PX -0.50
+param set-default CA_THRUSTER0_PY 0.50
+param set-default CA_THRUSTER0_PZ 0.0
+param set-default CA_THRUSTER0_CT 0.237
+param set-default CA_THRUSTER0_AX 0.0
+param set-default CA_THRUSTER0_AY -1.0
+param set-default CA_THRUSTER0_AZ 0.0
+
+param set-default CA_THRUSTER1_PX 0.50
+param set-default CA_THRUSTER1_PY 0.50
+param set-default CA_THRUSTER1_PZ 0.0
+param set-default CA_THRUSTER1_CT 0.237
+param set-default CA_THRUSTER1_AX 0.0
+param set-default CA_THRUSTER1_AY -1.0
+param set-default CA_THRUSTER1_AZ 0.0
+
+param set-default CA_THRUSTER2_PX 0.50
+param set-default CA_THRUSTER2_PY -0.50
+param set-default CA_THRUSTER2_PZ 0.0
+param set-default CA_THRUSTER2_CT 0.237
+param set-default CA_THRUSTER2_AX 0.0
+param set-default CA_THRUSTER2_AY 1.0
+param set-default CA_THRUSTER2_AZ 0.0
+
+param set-default CA_THRUSTER3_PX -0.50
+param set-default CA_THRUSTER3_PY -0.50
+param set-default CA_THRUSTER3_PZ 0.0
+param set-default CA_THRUSTER3_CT 0.237
+param set-default CA_THRUSTER3_AX 0.0
+param set-default CA_THRUSTER3_AY 1.0
+param set-default CA_THRUSTER3_AZ 0.0
+
+param set-default CA_THRUSTER4_PX -0.50
+param set-default CA_THRUSTER4_PY 0.0
+param set-default CA_THRUSTER4_PZ -0.50
+param set-default CA_THRUSTER4_CT 0.237
+param set-default CA_THRUSTER4_AX 1.0
+param set-default CA_THRUSTER4_AY 0.0
+param set-default CA_THRUSTER4_AZ 0.0
+
+param set-default CA_THRUSTER5_PX 0.50
+param set-default CA_THRUSTER5_PY 0.0
+param set-default CA_THRUSTER5_PZ -0.50
+param set-default CA_THRUSTER5_CT 0.237
+param set-default CA_THRUSTER5_AX -1.0
+param set-default CA_THRUSTER5_AY 0.0
+param set-default CA_THRUSTER5_AZ 0.0
+
+param set-default CA_THRUSTER6_PX 0.50
+param set-default CA_THRUSTER6_PY 0.0
+param set-default CA_THRUSTER6_PZ 0.50
+param set-default CA_THRUSTER6_CT 0.237
+param set-default CA_THRUSTER6_AX -1.0
+param set-default CA_THRUSTER6_AY 0.0
+param set-default CA_THRUSTER6_AZ 0.0
+
+param set-default CA_THRUSTER7_PX -0.50
+param set-default CA_THRUSTER7_PY 0.0
+param set-default CA_THRUSTER7_PZ 0.50
+param set-default CA_THRUSTER7_CT 0.237
+param set-default CA_THRUSTER7_AX 1.0
+param set-default CA_THRUSTER7_AY 0.0
+param set-default CA_THRUSTER7_AZ 0.0
+
+param set-default CA_THRUSTER8_PX 0.0
+param set-default CA_THRUSTER8_PY -0.50
+param set-default CA_THRUSTER8_PZ -0.50
+param set-default CA_THRUSTER8_CT 0.237
+param set-default CA_THRUSTER8_AX 0.0
+param set-default CA_THRUSTER8_AY 0.0
+param set-default CA_THRUSTER8_AZ 1.0
+
+param set-default CA_THRUSTER9_PX 0.0
+param set-default CA_THRUSTER9_PY 0.50
+param set-default CA_THRUSTER9_PZ -0.50
+param set-default CA_THRUSTER9_CT 0.237
+param set-default CA_THRUSTER9_AX 0.0
+param set-default CA_THRUSTER9_AY 0.0
+param set-default CA_THRUSTER9_AZ 1.0
+
+param set-default CA_THRUSTER10_PX 0.0
+param set-default CA_THRUSTER10_PY 0.50
+param set-default CA_THRUSTER10_PZ 0.50
+param set-default CA_THRUSTER10_CT 0.237
+param set-default CA_THRUSTER10_AX 0.0
+param set-default CA_THRUSTER10_AY 0.0
+param set-default CA_THRUSTER10_AZ -1.0
+
+param set-default CA_THRUSTER11_PX 0.0
+param set-default CA_THRUSTER11_PY -0.50
+param set-default CA_THRUSTER11_PZ 0.50
+param set-default CA_THRUSTER11_CT 0.237
+param set-default CA_THRUSTER11_AX 0.0
+param set-default CA_THRUSTER11_AY 0.0
+param set-default CA_THRUSTER11_AZ -1.0
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 105
+param set-default PWM_MAIN_FUNC6 106
+param set-default PWM_MAIN_FUNC7 107
+param set-default PWM_MAIN_FUNC8 108
+param set-default PWM_MAIN_FUNC9 109
+param set-default PWM_MAIN_FUNC10 110
+param set-default PWM_MAIN_FUNC11 111
+param set-default PWM_MAIN_FUNC12 112
+
+# PWM Simulation
+param set PWM_SIM_PWM_MAX 10000
+param set PWM_SIM_PWM_MIN 0
+
+# Controller Tunings
+param set-default SC_ROLLRATE_P 0.14
+param set-default SC_PITCHRATE_P 0.14
+param set-default SC_ROLLRATE_I 0.3
+param set-default SC_PITCHRATE_I 0.3
+param set-default SC_ROLLRATE_D 0.004
+param set-default SC_PITCHRATE_D 0.004

ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d

@@ -0,0 +1,149 @@
+#!/bin/sh
+#
+# @name 3DoF Spacecraft Model
+#
+# @type 2D Freeflyer with 8 thrusters - Planar motion
+#
+# @maintainer Pedro Roque <padr@kth.se>
+#
+
+. ${R}etc/init.d/rc.sc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d}
+
+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
+param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
+
+param set-default CA_AIRFRAME 14
+param set-default MAV_TYPE 99
+
+param set-default CA_THRUSTER_CNT 8
+param set-default CA_R_REV 0
+
+# param set-default FW_ARSP_MODE 1
+
+# Auto to be provided by Custom Airframe
+param set-default CA_METHOD 0  # 0 is PseudoInverse, 3 is Metric
+
+# Set proper failsafes
+param set-default COM_ACT_FAIL_ACT 0
+param set-default COM_LOW_BAT_ACT 0
+param set-default NAV_DLL_ACT 0
+param set-default GF_ACTION 1
+param set-default NAV_RCL_ACT 1
+param set-default COM_POSCTL_NAVL 2
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
+param set-default CA_THRUSTER0_PX -0.12
+param set-default CA_THRUSTER0_PY -0.12
+param set-default CA_THRUSTER0_PZ 0.0
+param set-default CA_THRUSTER0_CT 1.4
+param set-default CA_THRUSTER0_AX 1.0
+param set-default CA_THRUSTER0_AY 0.0
+param set-default CA_THRUSTER0_AZ 0.0
+
+param set-default CA_THRUSTER1_PX 0.12
+param set-default CA_THRUSTER1_PY -0.12
+param set-default CA_THRUSTER1_PZ 0.0
+param set-default CA_THRUSTER1_CT 1.4
+param set-default CA_THRUSTER1_AX -1.0
+param set-default CA_THRUSTER1_AY 0.0
+param set-default CA_THRUSTER1_AZ 0.0
+
+param set-default CA_THRUSTER2_PX -0.12
+param set-default CA_THRUSTER2_PY 0.12
+param set-default CA_THRUSTER2_PZ 0.0
+param set-default CA_THRUSTER2_CT 1.4
+param set-default CA_THRUSTER2_AX 1.0
+param set-default CA_THRUSTER2_AY 0.0
+param set-default CA_THRUSTER2_AZ 0.0
+
+param set-default CA_THRUSTER3_PX 0.12
+param set-default CA_THRUSTER3_PY 0.12
+param set-default CA_THRUSTER3_PZ 0.0
+param set-default CA_THRUSTER3_CT 1.4
+param set-default CA_THRUSTER3_AX -1.0
+param set-default CA_THRUSTER3_AY 0.0
+param set-default CA_THRUSTER3_AZ 0.0
+
+param set-default CA_THRUSTER4_PX 0.12
+param set-default CA_THRUSTER4_PY -0.12
+param set-default CA_THRUSTER4_PZ 0.0
+param set-default CA_THRUSTER4_CT 1.4
+param set-default CA_THRUSTER4_AX 0.0
+param set-default CA_THRUSTER4_AY 1.0
+param set-default CA_THRUSTER4_AZ 0.0
+
+param set-default CA_THRUSTER5_PX 0.12
+param set-default CA_THRUSTER5_PY 0.12
+param set-default CA_THRUSTER5_PZ 0.0
+param set-default CA_THRUSTER5_CT 1.4
+param set-default CA_THRUSTER5_AX 0.0
+param set-default CA_THRUSTER5_AY -1.0
+param set-default CA_THRUSTER5_AZ 0.0
+
+param set-default CA_THRUSTER6_PX -0.12
+param set-default CA_THRUSTER6_PY -0.12
+param set-default CA_THRUSTER6_PZ 0.0
+param set-default CA_THRUSTER6_CT 1.4
+param set-default CA_THRUSTER6_AX 0.0
+param set-default CA_THRUSTER6_AY 1.0
+param set-default CA_THRUSTER6_AZ 0.0
+
+param set-default CA_THRUSTER7_PX -0.12
+param set-default CA_THRUSTER7_PY 0.12
+param set-default CA_THRUSTER7_PZ 0.0
+param set-default CA_THRUSTER7_CT 1.4
+param set-default CA_THRUSTER7_AX 0.0
+param set-default CA_THRUSTER7_AY -1.0
+param set-default CA_THRUSTER7_AZ 0.0
+
+param set-default SIM_GZ_TH_FUNC1 101
+param set-default SIM_GZ_TH_FUNC2 102
+param set-default SIM_GZ_TH_FUNC3 103
+param set-default SIM_GZ_TH_FUNC4 104
+param set-default SIM_GZ_TH_FUNC5 105
+param set-default SIM_GZ_TH_FUNC6 106
+param set-default SIM_GZ_TH_FUNC7 107
+param set-default SIM_GZ_TH_FUNC8 108
+
+param set-default SIM_GZ_TH_MIN1 0
+param set-default SIM_GZ_TH_MIN2 0
+param set-default SIM_GZ_TH_MIN3 0
+param set-default SIM_GZ_TH_MIN4 0
+param set-default SIM_GZ_TH_MIN5 0
+param set-default SIM_GZ_TH_MIN6 0
+param set-default SIM_GZ_TH_MIN7 0
+param set-default SIM_GZ_TH_MIN8 0
+
+param set-default SIM_GZ_TH_MAX1 10000
+param set-default SIM_GZ_TH_MAX2 10000
+param set-default SIM_GZ_TH_MAX3 10000
+param set-default SIM_GZ_TH_MAX4 10000
+param set-default SIM_GZ_TH_MAX5 10000
+param set-default SIM_GZ_TH_MAX6 10000
+param set-default SIM_GZ_TH_MAX7 10000
+param set-default SIM_GZ_TH_MAX8 10000
+
+# Controller Tunings
+param set SC_YAWRATE_P 3.335
+param set SC_YAWRATE_I 0.87
+param set SC_YAWRATE_D 0.15
+param set SC_YR_INT_LIM 0.2
+param set SC_YAW_P 3.0
+
+param set SPC_POS_P 0.20
+param set SPC_VEL_P 6.55
+param set SPC_VEL_I 0.0
+param set SPC_VEL_D 0.0
+param set SPC_VEL_MAX 12.0

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

@@ -109,9 +109,13 @@ px4_add_romfs_files(
 	10040_sihsim_quadx
 	10041_sihsim_airplane
 	10042_sihsim_xvert
+	10043_sihsim_standard_vtol
 
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2
 
+	71001_gazebo-classic_spacecraft_dart
+	71002_gz_spacecraft_2d
+
 	# [22000, 22999] Reserve for custom models
 )

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -4,6 +4,9 @@
 # Simulator IMU data provided at 250 Hz
 param set-default IMU_INTEG_RATE 250
 
+# For simulation, allow registering modes while armed for developer convenience
+param set-default COM_MODE_ARM_CHK 1
+
 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
 
 	echo "INFO  [init] SIH simulator"

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
 	)
 endif()
 
+if(CONFIG_MODULES_SPACECRAFT)
+	px4_add_romfs_files(
+		rc.sc_apps
+		rc.sc_defaults
+	)
+endif()
+
 if(CONFIG_MODULES_ROVER_MECANUM)
 	px4_add_romfs_files(
 		rc.rover_mecanum_apps

ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil

@@ -17,15 +17,16 @@ param set UAVCAN_ENABLE 0
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
-param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
-param set-default CA_SV_CS2_TRQ_Y 1
-param set-default CA_SV_CS2_TYPE 4
 
-param set HIL_ACT_REV 1
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3  # elevator
+param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
 param set HIL_ACT_FUNC1 201
 param set HIL_ACT_FUNC2 202
 param set HIL_ACT_FUNC3 203

ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil

@@ -10,7 +10,7 @@
 # @output Motor3 MC motor front left
 # @output Motor4 MC motor back right
 # @output Motor5 Forward thrust motor
-# @output Servo1 Aileron
+# @output Servo1 Ailerons (single channel)
 # @output Servo2 Elevator
 # @output Servo3 Rudder
 #
@@ -20,8 +20,7 @@
 . ${R}etc/init.d/rc.vtol_defaults
 
 param set UAVCAN_ENABLE 0
-param set-default VT_B_TRANS_DUR 5
-param set-default VT_ELEV_MC_LOCK 0
+
 param set-default VT_TYPE 2
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5
@@ -40,25 +39,23 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.2
 param set-default CA_ROTOR3_PY 0.2
 param set-default CA_ROTOR3_KM -0.05
-
-
 param set-default CA_ROTOR4_PX -0.3
 param set-default CA_ROTOR4_KM 0.05
 param set-default CA_ROTOR4_AX 1
 param set-default CA_ROTOR4_AZ 0
 
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
 param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R 0.5
-param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
 param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS1_TYPE 3  # elevator
 param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
 
-param set HIL_ACT_REV 32
-
-param set-default FW_AIRSPD_MAX 12
 param set-default FW_AIRSPD_MIN 7
 param set-default FW_AIRSPD_TRIM 10
+param set-default FW_AIRSPD_MAX 12
 
 param set-default HIL_ACT_FUNC1 101
 param set-default HIL_ACT_FUNC2 102
@@ -71,16 +68,12 @@ param set-default HIL_ACT_FUNC8 105
 
 param set-default MAV_TYPE 22
 
-
-
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2
 
 # disable some checks to allow to fly:
 # - without real battery
 param set-default CBRK_SUPPLY_CHK 894281
-# - without safety switch
-param set-default CBRK_IO_SAFETY 22027
 
 param set-default SENS_DPRES_OFF 0.001
 
@@ -93,9 +86,7 @@ param set SIH_IYY 0.000625
 param set SIH_IZZ 0.00300
 param set SIH_IXZ 0
 param set SIH_KDV 0.2
-param set SIH_L_ROLL 0.145
+param set SIH_L_ROLL 0.2
 
 # sih as standard vtol
 param set SIH_VEHICLE_TYPE 3
-
-param set-default VT_ARSP_TRANS 6

ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d

@@ -0,0 +1,150 @@
+#!/bin/sh
+#
+# @name KTH Space Robot
+#
+# @type Space Robot
+# @class 2D Space Robot
+#
+# @maintainer DISCOWER
+#
+
+. ${R}etc/init.d/rc.sc_defaults
+
+param set-default CA_AIRFRAME 14
+param set-default MAV_TYPE 99
+
+param set-default CA_THRUSTER_CNT 8
+param set-default CA_R_REV 0
+
+# Auto to be provided by Custom Airframe
+param set-default CA_METHOD 0
+
+# Set proper failsafes
+param set-default COM_ACT_FAIL_ACT 0
+param set-default COM_LOW_BAT_ACT 0
+param set-default NAV_DLL_ACT 0
+param set-default GF_ACTION 1
+param set-default NAV_RCL_ACT 1
+param set-default COM_POSCTL_NAVL 2
+
+# Set Mocap Vision frame
+param set EKF2_EV_CTRL 15
+param set EKF2_HGT_REF 3
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
+param set-default CA_THRUSTER0_PX -0.12
+param set-default CA_THRUSTER0_PY -0.12
+param set-default CA_THRUSTER0_PZ 0.0
+param set-default CA_THRUSTER0_CT 1.4
+param set-default CA_THRUSTER0_AX 1.0
+param set-default CA_THRUSTER0_AY 0.0
+param set-default CA_THRUSTER0_AZ 0.0
+
+param set-default CA_THRUSTER1_PX 0.12
+param set-default CA_THRUSTER1_PY -0.12
+param set-default CA_THRUSTER1_PZ 0.0
+param set-default CA_THRUSTER1_CT 1.4
+param set-default CA_THRUSTER1_AX -1.0
+param set-default CA_THRUSTER1_AY 0.0
+param set-default CA_THRUSTER1_AZ 0.0
+
+param set-default CA_THRUSTER2_PX -0.12
+param set-default CA_THRUSTER2_PY 0.12
+param set-default CA_THRUSTER2_PZ 0.0
+param set-default CA_THRUSTER2_CT 1.4
+param set-default CA_THRUSTER2_AX 1.0
+param set-default CA_THRUSTER2_AY 0.0
+param set-default CA_THRUSTER2_AZ 0.0
+
+param set-default CA_THRUSTER3_PX 0.12
+param set-default CA_THRUSTER3_PY 0.12
+param set-default CA_THRUSTER3_PZ 0.0
+param set-default CA_THRUSTER3_CT 1.4
+param set-default CA_THRUSTER3_AX -1.0
+param set-default CA_THRUSTER3_AY 0.0
+param set-default CA_THRUSTER3_AZ 0.0
+
+param set-default CA_THRUSTER4_PX 0.12
+param set-default CA_THRUSTER4_PY -0.12
+param set-default CA_THRUSTER4_PZ 0.0
+param set-default CA_THRUSTER4_CT 1.4
+param set-default CA_THRUSTER4_AX 0.0
+param set-default CA_THRUSTER4_AY 1.0
+param set-default CA_THRUSTER4_AZ 0.0
+
+param set-default CA_THRUSTER5_PX 0.12
+param set-default CA_THRUSTER5_PY 0.12
+param set-default CA_THRUSTER5_PZ 0.0
+param set-default CA_THRUSTER5_CT 1.4
+param set-default CA_THRUSTER5_AX 0.0
+param set-default CA_THRUSTER5_AY -1.0
+param set-default CA_THRUSTER5_AZ 0.0
+
+param set-default CA_THRUSTER6_PX -0.12
+param set-default CA_THRUSTER6_PY -0.12
+param set-default CA_THRUSTER6_PZ 0.0
+param set-default CA_THRUSTER6_CT 1.4
+param set-default CA_THRUSTER6_AX 0.0
+param set-default CA_THRUSTER6_AY 1.0
+param set-default CA_THRUSTER6_AZ 0.0
+
+param set-default CA_THRUSTER7_PX -0.12
+param set-default CA_THRUSTER7_PY 0.12
+param set-default CA_THRUSTER7_PZ 0.0
+param set-default CA_THRUSTER7_CT 1.4
+param set-default CA_THRUSTER7_AX 0.0
+param set-default CA_THRUSTER7_AY -1.0
+param set-default CA_THRUSTER7_AZ 0.0
+
+
+param set-default PWM_AUX_TIM0 10
+param set-default PWM_AUX_TIM1 10
+param set-default PWM_AUX_TIM2 10
+
+param set-default PWM_AUX_FUNC1 101
+param set-default PWM_AUX_FUNC2 102
+param set-default PWM_AUX_FUNC3 103
+param set-default PWM_AUX_FUNC4 104
+param set-default PWM_AUX_FUNC5 105
+param set-default PWM_AUX_FUNC6 106
+param set-default PWM_AUX_FUNC7 107
+param set-default PWM_AUX_FUNC8 108
+
+param set-default PWM_AUX_DIS1 0
+param set-default PWM_AUX_DIS2 0
+param set-default PWM_AUX_DIS3 0
+param set-default PWM_AUX_DIS4 0
+param set-default PWM_AUX_DIS5 0
+param set-default PWM_AUX_DIS6 0
+param set-default PWM_AUX_DIS7 0
+param set-default PWM_AUX_DIS8 0
+
+param set-default PWM_AUX_MIN1 0
+param set-default PWM_AUX_MIN2 0
+param set-default PWM_AUX_MIN3 0
+param set-default PWM_AUX_MIN4 0
+param set-default PWM_AUX_MIN5 0
+param set-default PWM_AUX_MIN6 0
+param set-default PWM_AUX_MIN7 0
+param set-default PWM_AUX_MIN8 0
+
+# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec
+param set-default PWM_AUX_MAX1 10000
+param set-default PWM_AUX_MAX2 10000
+param set-default PWM_AUX_MAX3 10000
+param set-default PWM_AUX_MAX4 10000
+param set-default PWM_AUX_MAX5 10000
+param set-default PWM_AUX_MAX6 10000
+param set-default PWM_AUX_MAX7 10000
+param set-default PWM_AUX_MAX8 10000
+
+# Controller Tunings
+param set-default SC_ROLLRATE_P 0.14
+param set-default SC_PITCHRATE_P 0.14
+param set-default SC_ROLLRATE_I 0.3
+param set-default SC_PITCHRATE_I 0.3
+param set-default SC_ROLLRATE_D 0.004
+param set-default SC_PITCHRATE_D 0.004

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -24,7 +24,6 @@ param set-default COM_DISARM_PRFLT -1
 
 param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
-param set-default EKF2_GPS_CHECK 21
 param set-default EKF2_MAG_ACCLIM 0
 param set-default EKF2_REQ_EPH 10
 param set-default EKF2_REQ_EPV 10

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -21,3 +21,6 @@ param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10
 
 param set-default GPS_UBX_DYNMODEL 6
+
+# lower RNG_FOG since MC are expected to fly closer over obstacles
+param set-default EKF2_RNG_FOG 1.0

ROMFS/px4fmu_common/init.d/rc.sc_apps

@@ -0,0 +1,36 @@
+#!/bin/sh
+#
+# Standard apps for sr. Attitude/Position estimator, Attitude/Position control.
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+# Start Spacecraft App
+spacecraft start
+
+# Estimator Group Selection
+# ekf2 start &
+
+# Start MicroDDS Client
+# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2
+# uxrce_dds_client start -t udp -p 8888
+
+#
+# Start Control Allocator
+#
+# sc_control_allocator start
+
+#
+# Start Spacecraft Rate Controller.
+#
+# sc_rate_control start
+
+#
+# Start Spacecraft Attitude Controller.
+#
+# sc_att_control start
+
+#
+# Start Spacecraft Position Controller.
+#
+# sc_pos_control start

ROMFS/px4fmu_common/init.d/rc.sc_defaults

@@ -0,0 +1,27 @@
+#!/bin/sh
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+set VEHICLE_TYPE sc
+
+# MAV_TYPE_QUADROTOR 2
+#param set-default MAV_TYPE 12
+
+# Set micro-dds-client to use ethernet and IP-address 192.168.0.1
+param set-default UXRCE_DDS_AG_IP -1062731775
+
+# Disable preflight disarm to not interfere with external launching
+param set-default COM_DISARM_PRFLT -1
+param set-default CBRK_SUPPLY_CHK 894281
+param set-default COM_ARM_HFLT_CHK 0
+
+#Missing params
+param set-default CP_DIST -1.0
+
+# Default to MoCap fusion
+param set-default ATT_EXT_HDG_M 2
+param set-default EKF2_EV_CTRL 15
+param set-default EKF2_EV_DELAY 5
+param set-default EKF2_GPS_CTRL 0
+param set-default EKF2_HGT_REF 3

ROMFS/px4fmu_common/init.d/rcS

@@ -302,6 +302,75 @@ else
 		. $FCONFIG
 	fi
 
+
+	#
+	# Sensors System (start before Commander so Preflight checks are properly run).
+	#
+	if param greater SYS_HITL 0
+	then
+		sensors start -h
+
+		# disable GPS
+		param set GPS_1_CONFIG 0
+
+		# start the simulator in hardware if needed
+		if param compare SYS_HITL 2
+		then
+			simulator_sih start
+			sensor_baro_sim start
+			sensor_mag_sim start
+			sensor_gps_sim start
+			sensor_agp_sim start
+		fi
+
+	else
+		#
+		# board sensors: rc.sensors
+		#
+		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
+		if [ -f $BOARD_RC_SENSORS ]
+		then
+			echo "Board sensors: ${BOARD_RC_SENSORS}"
+			. $BOARD_RC_SENSORS
+		fi
+		unset BOARD_RC_SENSORS
+
+		. ${R}etc/init.d/rc.sensors
+
+		if param compare -s BAT1_SOURCE 2
+		then
+			esc_battery start
+		fi
+
+		if ! param compare BAT1_SOURCE 1
+		then
+			battery_status start
+		fi
+
+		sensors start
+	fi
+
+	#
+	# state estimator selection
+	#
+	if param compare -s EKF2_EN 1
+	then
+		ekf2 start &
+	fi
+
+	if param compare -s LPE_EN 1
+	then
+		local_position_estimator start
+	fi
+
+	if param compare -s ATT_EN 1
+	then
+		attitude_estimator_q start
+	fi
+
+	#
+	# px4io
+	#
 	if px4io supported
 	then
 	# Check if PX4IO present and update firmware if needed.
@@ -369,79 +438,24 @@ else
 	fi
 
 	#
-	# Sensors System (start before Commander so Preflight checks are properly run).
-	# Commander needs to be this early for in-air-restarts.
+	# Commander
 	#
 	if param greater SYS_HITL 0
 	then
+		commander start -h
+
 		if ! pwm_out_sim start -m hil
 		then
 			tune_control play error
 		fi
 
-		sensors start -h
-		commander start -h
-		# disable GPS
-		param set GPS_1_CONFIG 0
-
-		# start the simulator in hardware if needed
-		if param compare SYS_HITL 2
-		then
-			simulator_sih start
-			sensor_baro_sim start
-			sensor_mag_sim start
-			sensor_gps_sim start
-			sensor_agp_sim start
-		fi
-
 	else
-		#
-		# board sensors: rc.sensors
-		#
-		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
-		if [ -f $BOARD_RC_SENSORS ]
-		then
-			echo "Board sensors: ${BOARD_RC_SENSORS}"
-			. $BOARD_RC_SENSORS
-		fi
-		unset BOARD_RC_SENSORS
-
-		. ${R}etc/init.d/rc.sensors
-
-		if param compare -s BAT1_SOURCE 2
-		then
-			esc_battery start
-		fi
-
-		if ! param compare BAT1_SOURCE 1
-		then
-			battery_status start
-		fi
-
-		sensors start
 		commander start
 
 		dshot start
 		pwm_out start
 	fi
 
-	#
-	# state estimator selection
-	if param compare -s EKF2_EN 1
-	then
-		ekf2 start &
-	fi
-
-	if param compare -s LPE_EN 1
-	then
-		local_position_estimator start
-	fi
-
-	if param compare -s ATT_EN 1
-	then
-		attitude_estimator_q start
-	fi
-
 	#
 	# Configure vehicle type specific parameters.
 	# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

Tools/kconfig/allyesconfig.py

@@ -53,6 +53,7 @@ exception_list = [
     'SYSTEMCMDS_I2C_LAUNCHER', #  undefined reference to `system',
     'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl
     'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors
+    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]
 
 exception_list_sitl = [
@@ -73,6 +74,7 @@ exception_list_sitl = [
     'SYSTEMCMDS_I2CDETECT', # Not supported in SITL
     'SYSTEMCMDS_DMESG', # Not supported in SITL
     'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL
+    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]
 
 def main():

Tools/px_uploader.py

@@ -189,7 +189,7 @@ class uploader:
     GET_CHIP        = b'\x2c'     # rev5+  , get chip version
     SET_BOOT_DELAY  = b'\x2d'     # rev5+  , set boot delay
     GET_CHIP_DES    = b'\x2e'     # rev5+  , get chip description in ASCII
-    GET_VERSION     = b'\x2f'     # rev5+  , get chip description in ASCII
+    GET_VERSION     = b'\x2f'     # rev5+  , get bootloader version in ASCII
     CHIP_FULL_ERASE = b'\x40'     # full erase of flash, rev6+
     MAX_DES_LENGTH  = 20
 
@@ -201,7 +201,6 @@ class uploader:
     INFO_BOARD_ID   = b'\x02'        # board type
     INFO_BOARD_REV  = b'\x03'        # board revision
     INFO_FLASH_SIZE = b'\x04'        # max firmware size in bytes
-    BL_VERSION      = b'\x07'        # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars)
 
     PROG_MULTI_MAX  = 252            # protocol max is 255, must be multiple of 4
     READ_MULTI_MAX  = 252            # protocol max is 255

boards/ark/can-flow-mr/canbootloader.px4board

@@ -0,0 +1,5 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m4"
+CONFIG_BOARD_ROMFSROOT=""
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_DRIVERS_BOOTLOADERS=y

boards/ark/can-flow-mr/firmware.prototype

@@ -0,0 +1,13 @@
+{
+    "board_id": 88,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the ARK Flow MR board",
+    "image": "",
+    "build_time": 0,
+    "summary": "ARKFLOWMR",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 2080768,
+    "git_identity": "",
+    "board_revision": 0
+}

boards/ark/can-flow-mr/init/rc.board_sensors

@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# board sensors init
+#------------------------------------------------------------------------------
+
+param set-default IMU_GYRO_RATEMAX 1000
+param set-default SENS_FLOW_RATE 150
+param set-default SENS_IMU_CLPNOTI 0
+
+param set-default SENS_AFBR_S_RATE 25
+param set-default SENS_AFBR_L_RATE 10
+param set-default SENS_AFBR_THRESH 8
+param set-default SENS_AFBR_HYSTER 2
+
+# Internal SPI
+paa3905 -s start -Y 180
+
+iim42653 -R 0 -s start
+
+afbrs50 start

boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig

@@ -0,0 +1,57 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F412CE=y
+CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BINFMT_DISABLE=y
+CONFIG_BOARDCTL=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MOUNTPOINT=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=4096
+CONFIG_INIT_STACKSIZE=4096
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MM_REGIONS=2
+CONFIG_NAME_MAX=0
+CONFIG_NUNGET_CHARS=0
+CONFIG_PREALLOC_TIMERS=0
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=262144
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_DISABLE_BUFFERING=y
+CONFIG_STM32_FLASH_CONFIG_G=y
+CONFIG_STM32_NOEXT_VECTORS=y
+CONFIG_STM32_TIM8=y
+CONFIG_TASK_NAME_SIZE=0
+CONFIG_USEC_PER_TICK=1000

boards/ark/can-flow-mr/nuttx-config/include/board.h

@@ -0,0 +1,136 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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 "board_dma_map.h"
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/* HSI - 8 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - 8  MHz Crystal
+ * LSE - not installed
+ */
+#define STM32_BOARD_USEHSE      1
+#define STM32_BOARD_XTAL        8000000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+
+/* Main PLL Configuration */
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(8)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(384)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_4
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(8)
+#define STM32_PLLCFG_PLLR       RCC_PLLCFG_PLLR(2)
+
+#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/
+
+#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL
+#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB
+#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ
+
+#define STM32_SYSCLK_FREQUENCY  96000000ul
+
+/* AHB clock (HCLK) is SYSCLK (96MHz) */
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK      /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY      /* Same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd2     /* PCLK1 = HCLK / 2 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK (96MHz) */
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK       /* PCLK2 = HCLK */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* Timers driven from APB2 will be PCLK2 since no prescale division */
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */
+#define BOARD_TIM2_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM3_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM4_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM5_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM6_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM7_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM8_FREQUENCY    (2 * STM32_PCLK2_FREQUENCY)
+
+/* Alternate function pin selections ************************************************/
+
+/* UARTs */
+#define GPIO_USART2_RX	GPIO_USART2_RX_1
+#define GPIO_USART2_TX	GPIO_USART2_TX_1
+
+/* CAN */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_1
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_1
+
+/* SPI */
+#define GPIO_SPI1_MISO  GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI  GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK   GPIO_SPI1_SCK_1
+
+#define GPIO_SPI2_MISO  GPIO_SPI2_MISO_1 /* PB14 */
+#define GPIO_SPI2_MOSI  GPIO_SPI2_MOSI_1 /* PB15 */
+#define GPIO_SPI2_SCK   GPIO_SPI2_SCK_1  /* PB10 */
+
+#endif /* __ARCH_BOARD_BOARD_H */

boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,44 +31,14 @@
  *
  ****************************************************************************/
 
-/**
- * @file ControlAllocationMetric.hpp
- *
- * Control allocation with metric units.
- *
- * The metric allocation takes into account maximum thrust and torque
- * provided by the actuators to calculate setpoints that allow to allocate
- * specific body force and torque, keeping the scale of the target setpoints.
- * No normalization is done in this case. Note that, similarly to the PseudoInverse
- * allocation, saturation is handled by clipping.
- *
- * @author Pedro Roque <padr@kth.se>
- */
-
 #pragma once
 
-#include "ControlAllocation.hpp"
+// DMA1 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+#define DMACHAN_SPI2_RX    DMAMAP_SPI2_RX   // DMA1, Stream 3, Channel 0
+#define DMACHAN_SPI2_TX    DMAMAP_SPI2_TX   // DMA1, Stream 4, Channel 0
 
-class ControlAllocationMetric: public ControlAllocation
-{
-public:
-	ControlAllocationMetric() = default;
-	virtual ~ControlAllocationMetric() = default;
-
-	void allocate() override;
-	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
-				    bool update_normalization_scale) override;
-
-protected:
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
-
-	bool _mix_update_needed{false};
-
-	/**
-	 * Recalculate pseudo inverse if required.
-	 *
-	 */
-	void updatePseudoInverse();
-
-};
+//  DMA2 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+#define DMACHAN_SPI1_RX    DMAMAP_SPI1_RX_2   // DMA2, Stream 2, Channel 3
+#define DMACHAN_SPI1_TX    DMAMAP_SPI1_TX_2   // DMA2, Stream 5, Channel 3

boards/ark/can-flow-mr/nuttx-config/nsh/defconfig

@@ -0,0 +1,149 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F412CE=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=2624
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MM_REGIONS=2
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_VARS=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=262144
+CONFIG_RAM_START=0x20000000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=254
+CONFIG_SCHED_HPWORKSTACKSIZE=3000
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_WAITPID=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+CONFIG_STM32_FLASH_CONFIG_G=y
+CONFIG_STM32_FLASH_PREFETCH=y
+CONFIG_STM32_FLOWCONTROL_BROKEN=y
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_PWR=y
+CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
+CONFIG_STM32_SPI1_DMA_BUFFER=2048
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI2_DMA=y
+CONFIG_STM32_SPI2_DMA_BUFFER=2048
+CONFIG_STM32_TIM8=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART_BREAKS=y
+CONFIG_STM32_WWDG=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_SERIAL_CONSOLE=y
+CONFIG_USART2_TXBUFSIZE=1100
+CONFIG_USEC_PER_TICK=1000

boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld

@@ -0,0 +1,134 @@
+/****************************************************************************
+ * nuttx-config/scripts/canbootloader_script.ld
+ *
+ *   Copyright (C) 2015 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x10000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08000000, LENGTH = 32K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/ark/can-flow-mr/nuttx-config/scripts/script.ld

@@ -0,0 +1,146 @@
+/****************************************************************************
+ * scripts/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x10000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08010000, LENGTH = 928K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(8);
+		/*
+		* This section positions the app_descriptor_t used
+		* by the make_can_boot_descriptor.py tool to set
+		* the application image's descriptor so that the
+		* uavcan bootloader has the ability to validate the
+		* image crc, size etc
+		*/
+		KEEP(*(.app_descriptor))
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/ark/can-flow-mr/src/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,28 +31,35 @@
 #
 ############################################################################
 
-px4_add_library(ActuatorEffectiveness
-	ActuatorEffectiveness.cpp
-	ActuatorEffectiveness.hpp
-<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
-	ActuatorEffectivenessRotors.cpp
-	ActuatorEffectivenessRotors.hpp
-	ActuatorEffectivenessThrusters.cpp
-	ActuatorEffectivenessThrusters.hpp
-========
->>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
-)
+if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
 
-target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-target_link_libraries(ActuatorEffectiveness
-	PRIVATE
-		mathlib
-		PID
-)
-<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt
+	add_library(drivers_board
+		boot_config.h
+		boot.c
+		led.c
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch
+			nuttx_drivers
+			canbootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)
 
-px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
-px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness)
-========
->>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
+else()
+	add_library(drivers_board
+		can.c
+		init.c
+		led.c
+		spi.cpp
+	)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_spi
+			drivers__led # drv_led_start
+			nuttx_arch
+			nuttx_drivers
+			px4_layer
+	)
+endif()

boards/ark/can-flow-mr/src/board_config.h

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * board internal definitions
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* CAN  Silent mode control */
+#define GPIO_CAN1_SILENT_S0   /* PB11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11)
+
+/* CAN termination software control */
+#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_CAN_TERM                    GPIO_CAN1_TERMINATION
+
+/* Boot config */
+#define GPIO_BOOT_CONFIG      /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI)
+
+/* LEDs are driven with open drain to support Anode to 5V or 3.3V */
+#define GPIO_TIM1_CH1         /* PA8  */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+#define GPIO_TIM1_CH2         /* PA9  */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+#define GPIO_TIM1_CH3         /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+
+#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 2
+#define BROADCOM_AFBR_S50_S2PI_CS   /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12)
+#define BROADCOM_AFBR_S50_S2PI_IRQ  /* PB4  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4|GPIO_EXTI)
+#define BROADCOM_AFBR_S50_S2PI_CLK  /* PB10 */  GPIO_SPI2_SCK_1
+#define BROADCOM_AFBR_S50_S2PI_MOSI /* PB15 */  GPIO_SPI2_MOSI_1
+#define BROADCOM_AFBR_S50_S2PI_MISO /* PB14 */  GPIO_SPI2_MISO_1
+
+#define FLASH_BASED_PARAMS
+
+/* High-resolution timer */
+#define HRT_TIMER                    3  /* use timer 3 for the HRT */
+#define HRT_TIMER_CHANNEL            4  /* use capture/compare channel 4 */
+
+#define PX4_GPIO_INIT_LIST { \
+		GPIO_BOOT_CONFIG,                 \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_CAN1_SILENT_S0,              \
+		GPIO_CAN1_TERMINATION,            \
+	}
+
+__BEGIN_DECLS
+
+#define BOARD_HAS_N_S_RGB_LED       1
+#define BOARD_MAX_LEDS              BOARD_HAS_N_S_RGB_LED
+
+#ifndef __ASSEMBLY__
+
+extern void stm32_spiinitialize(void);
+
+#include <px4_platform_common/board_common.h>
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

boards/ark/can-flow-mr/src/boot.c

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *       Author: Ben Dyer <ben_dyer@mac.com>
+ *               Pavel Kirienko <pavel.kirienko@zubax.com>
+ *               David Sidrane <david_s5@nscdg.com>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <stdint.h>
+#include "boot_config.h"
+#include "board.h"
+
+#include <debug.h>
+#include <string.h>
+#include <arch/board/board.h>
+
+#include <nuttx/board.h>
+#include "led.h"
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the initialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR);
+	stm32_configgpio(GPIO_CAN1_RX);
+	stm32_configgpio(GPIO_CAN1_TX);
+	stm32_configgpio(GPIO_CAN1_SILENT_S0);
+	stm32_configgpio(GPIO_CAN1_TERMINATION);
+	putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+	putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+
+#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
+	stm32_configgpio(GPIO_GETNODEINFO_JUMPER);
+#endif
+
+}
+
+/************************************************************************************
+ * Name: board_deinitialize
+ *
+ * Description:
+ *   This function is called by the bootloader code prior to booting
+ *   the application. Is should place the HW into an benign initialized state.
+ *
+ ************************************************************************************/
+
+void board_deinitialize(void)
+{
+	putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+}
+
+/****************************************************************************
+ * Name: board_get_product_name
+ *
+ * Description:
+ *   Called to retrieve the product name. The returned value is a assumed
+ *   to be written to a pascal style string that will be length prefixed
+ *   and not null terminated
+ *
+ * Input Parameters:
+ *    product_name - A pointer to a buffer to write the name.
+ *    maxlen       - The maximum number of charter that can be written
+ *
+ * Returned Value:
+ *   The length of characters written to the buffer.
+ *
+ ****************************************************************************/
+
+uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen)
+{
+	DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME));
+	memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME));
+	return UAVCAN_STRLEN(HW_UAVCAN_NAME);
+}
+
+/****************************************************************************
+ * Name: board_get_hardware_version
+ *
+ * Description:
+ *   Called to retrieve the hardware version information. The function
+ *   will first initialize the the callers struct to all zeros.
+ *
+ * Input Parameters:
+ *    hw_version - A pointer to a uavcan_hardwareversion_t.
+ *
+ * Returned Value:
+ *   Length of the unique_id
+ *
+ ****************************************************************************/
+
+size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version)
+{
+	memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t));
+
+	hw_version->major = HW_VERSION_MAJOR;
+	hw_version->minor = HW_VERSION_MINOR;
+
+	return board_get_mfguid(*(mfguid_t *) hw_version->unique_id);
+}
+
+/****************************************************************************
+ * Name: board_indicate
+ *
+ * Description:
+ *   Provides User feedback to indicate the state of the bootloader
+ *   on board specific  hardware.
+ *
+ * Input Parameters:
+ *    indication - A member of the uiindication_t
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)}
+
+typedef begin_packed_struct struct led_t {
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+	uint8_t hz;
+} end_packed_struct led_t;
+
+static const  led_t i2l[] = {
+
+	led(0, off,                             0,     0,     0,     0),
+	led(1, reset,                          128,   128,   128,   30),
+	led(2, autobaud_start,                  0,   128,     0,     1),
+	led(3, autobaud_end,                    0,   128,     0,     2),
+	led(4, allocation_start,                0,     0,    64,     2),
+	led(5, allocation_end,                  0,   128,    64,     3),
+	led(6, fw_update_start,                32,   128,    64,     3),
+	led(7, fw_update_erase_fail,           32,   128,    32,     3),
+	led(8, fw_update_invalid_response,     64,     0,     0,     1),
+	led(9, fw_update_timeout,              64,     0,     0,     2),
+	led(a, fw_update_invalid_crc,          64,     0,     0,     4),
+	led(b, jump_to_app,                     0,   128,     0,    10),
+
+};
+
+void board_indicate(uiindication_t indication)
+{
+	rgb_led(i2l[indication].red,
+		i2l[indication].green,
+		i2l[indication].blue,
+		i2l[indication].hz);
+}

boards/ark/can-flow-mr/src/boot_config.h

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file boot_config.h
+ *
+ * bootloader definitions that configures the behavior and options
+ * of the Boot loader
+ * This file is relies on the parent folder's boot_config.h file and defines
+ * different usages of the hardware for bootloading
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+/* Bring in the board_config.h definitions
+ * todo:make this be pulled in from a targed's build
+ * files in nuttx*/
+
+#include "board_config.h"
+#include "uavcan.h"
+#include <nuttx/compiler.h>
+
+#include <stdint.h>
+
+#include <stm32_flash.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define OPT_PREFERRED_NODE_ID ANY_NODE_ID
+
+//todo:wrap OPT_x in in ifdefs for command line definitions
+#define OPT_TBOOT_MS                    3000
+#define OPT_NODE_STATUS_RATE_MS         800
+#define OPT_NODE_INFO_RATE_MS           50
+#define OPT_BL_NUMBER_TIMERS            7
+
+/*
+ *  This Option set is set to 1 ensure a provider of firmware has an
+ *  opportunity update the node's firmware.
+ *  This Option is the default policy and can be overridden by
+ *  a jumper
+ *  When this Policy is set, the node will ignore tboot and
+ *  wait indefinitely for a GetNodeInfo request before booting.
+ *
+ *  OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow
+ *  the polarity of the jumper to be True Active
+ *
+ *  wait  OPT_WAIT_FOR_GETNODEINFO  OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO
+ *                                                 Jumper
+ *   yes           1                       0         x
+ *   yes           1                       1       Active
+ *   no            1                       1       Not Active
+ *   no            0                       0         X
+ *   yes           0                       1       Active
+ *   no            0                       1       Not Active
+ *
+ */
+#define OPT_WAIT_FOR_GETNODEINFO                    0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
+
+#define OPT_ENABLE_WD           1
+
+#define OPT_RESTART_TIMEOUT_MS          20000
+
+/* Reserved for the Booloader */
+#define OPT_BOOTLOADER_SIZE_IN_K        (1024*64)
+
+/* Reserved for the application out of the total
+ * system flash minus the BOOTLOADER_SIZE_IN_K
+ */
+#define OPT_APPLICATION_RESERVER_IN_K    0
+
+#define OPT_APPLICATION_IMAGE_OFFSET    OPT_BOOTLOADER_SIZE_IN_K
+#define OPT_APPLICATION_IMAGE_LENGTH    (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K))
+
+
+#define FLASH_BASE              STM32_FLASH_BASE
+#define FLASH_SIZE              STM32_FLASH_SIZE
+
+#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET)
+#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET)
+#define APPLICATION_LAST_8BIT_ADDRRESS  ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t)))
+#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t)))
+#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t)))
+
+/* If this board uses big flash that have large sectors */
+
+#define OPT_USE_YIELD
+
+/* Bootloader Option*****************************************************************
+ *
+ */
+#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI)

boards/ark/can-flow-mr/src/can.c

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "arm_internal.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+int can_devinit(void);
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			canerr("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			canerr("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif

boards/ark/can-flow-mr/src/init.c

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * board specific early startup code.  This file implements the
+ * board_app_initialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/board.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include "led.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+#include <drivers/drv_watchdog.h>
+
+#include <systemlib/px4_macros.h>
+
+#include <px4_platform_common/init.h>
+#include <px4_platform/gpio.h>
+
+# if defined(FLASH_BASED_PARAMS)
+#  include <parameters/flashparams/flashfs.h>
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the initialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	watchdog_init();
+
+	/* configure pins */
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+
+	// Configure SPI all interfaces GPIO & enable power.
+	stm32_spiinitialize();
+}
+
+/****************************************************************************
+ * Name: board_app_initialize
+ *
+ * Description:
+ *   Perform application specific initialization.  This function is never
+ *   called directly from application code, but only indirectly via the
+ *   (non-standard) boardctl() interface using the command BOARDIOC_INIT.
+ *
+ * Input Parameters:
+ *   arg - The boardctl() argument is passed to the board_app_initialize()
+ *         implementation without modification.  The argument has no
+ *         meaning to NuttX; the meaning of the argument is a contract
+ *         between the board-specific initalization logic and the the
+ *         matching application logic.  The value cold be such things as a
+ *         mode enumeration value, a set of DIP switch switch settings, a
+ *         pointer to configuration data read from a file or serial FLASH,
+ *         or whatever you would like to do with it.  Every implementation
+ *         should accept zero/NULL as a default configuration.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success; a negated errno value is returned on
+ *   any failure to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+	px4_platform_init();
+
+#if defined(FLASH_BASED_PARAMS)
+	static sector_descriptor_t params_sector_map[] = {
+		{2, 16 * 1024, 0x08008000},
+		{3, 16 * 1024, 0x0800C000},
+		{0, 0, 0},
+	};
+
+	/* Initialize the flashfs layer to use heap allocated memory */
+	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
+
+	if (result != OK) {
+		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
+	}
+
+#endif // FLASH_BASED_PARAMS
+
+	/* Configure the HW based on the manifest */
+	//px4_platform_configure();
+
+	return OK;
+}

boards/ark/can-flow-mr/src/led.c

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file led.c
+ *
+ * LED backend.
+ */
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+#include "led.h"
+
+#define TMR_BASE        STM32_TIM1_BASE
+#define TMR_FREQUENCY   STM32_APB2_TIM1_CLKIN
+#define TMR_REG(o)      (TMR_BASE+(o))
+
+void rgb_led(int r, int g, int b, int freqs)
+{
+
+	long fosc = TMR_FREQUENCY;
+	long prescale = 2048;
+	long p1s = fosc / prescale;
+	long p0p5s  = p1s / 2;
+	uint16_t val;
+	static bool once = 0;
+
+	if (!once) {
+		once = 1;
+
+		/* Enabel Clock to Block */
+		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);
+
+		/* Reload */
+
+		val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET));
+		val |= ATIM_EGR_UG;
+		putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET));
+
+		/* Set Prescaler STM32_TIM_SETCLOCK */
+
+		putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET));
+
+		/* Enable STM32_TIM_SETMODE*/
+
+		putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+
+		putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE |
+			 (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET));
+		putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET));
+		putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P |
+			 ATIM_CCER_CC2E | ATIM_CCER_CC2P |
+			 ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET));
+
+
+		stm32_configgpio(GPIO_TIM1_CH1);
+		stm32_configgpio(GPIO_TIM1_CH2);
+		stm32_configgpio(GPIO_TIM1_CH3);
+
+		/* master output enable = on */
+		putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET)));
+	}
+
+	long p  = freqs == 0 ? p1s : p1s / freqs;
+	putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET));
+
+	p  = freqs == 0 ? p1s + 1 : p0p5s / freqs;
+
+	putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET));
+	putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET));
+	putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET));
+
+	val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+	if (freqs == 0) {
+		val &= ~ATIM_CR1_CEN;
+
+	} else {
+		val |= ATIM_CR1_CEN;
+	}
+
+	putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+}

boards/ark/can-flow-mr/src/led.h

@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *   Author: David Sidrane<david_s5@nscdg.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,46 +32,6 @@
  *
  ****************************************************************************/
 
-/**
- * @file ControlAllocationPseudoInverse.hpp
- *
- * Simple Control Allocation Algorithm
- *
- * It computes the pseudo-inverse of the effectiveness matrix
- * Actuator saturation is handled by simple clipping, do not
- * expect good performance in case of actuator saturation.
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#pragma once
-
-#include "ControlAllocation.hpp"
-
-class ControlAllocationPseudoInverse: public ControlAllocation
-{
-public:
-	ControlAllocationPseudoInverse() = default;
-	virtual ~ControlAllocationPseudoInverse() = default;
-
-	void allocate() override;
-	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
-				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
-				    bool update_normalization_scale) override;
-
-protected:
-	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
-
-	bool _mix_update_needed{false};
-
-	/**
-	 * Recalculate pseudo inverse if required.
-	 *
-	 */
-	void updatePseudoInverse();
-
-private:
-	void normalizeControlAllocationMatrix();
-	void updateControlAllocationMatrixScale();
-	bool _normalization_needs_update{false};
-};
+__BEGIN_DECLS
+void rgb_led(int r, int g, int b, int freqs);
+__END_DECLS

boards/ark/can-flow-mr/src/spi.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,45 +31,18 @@
  *
  ****************************************************************************/
 
-/**
- * @file ControlAllocationMetric.hpp
- *
- * Control allocation with metric units.
- *
- * @author Pedro Roque <padr@kth.se>
- */
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+#include <nuttx/spi/spi.h>
 
-#include "ControlAllocationMetric.hpp"
-#include <px4_platform_common/log.h>
+constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
+	initSPIBus(SPI::Bus::SPI1, {
+		initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}),
+		initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
+	}),
+	initSPIBus(SPI::Bus::SPI2, {
+		initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin12}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
+	}),
+};
 
-void
-ControlAllocationMetric::setEffectivenessMatrix(
-	const matrix::Matrix<float, ControlAllocation::NUM_AXES, ControlAllocation::NUM_ACTUATORS> &effectiveness,
-	const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
-	bool update_normalization_scale)
-{
-	ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators,
-			update_normalization_scale);
-	_mix_update_needed = true;
-}
-
-void
-ControlAllocationMetric::updatePseudoInverse()
-{
-	if (_mix_update_needed) {
-		matrix::geninv(_effectiveness, _mix);
-		_mix_update_needed = false;
-	}
-}
-
-void
-ControlAllocationMetric::allocate()
-{
-	//Compute new gains if needed
-	updatePseudoInverse();
-
-	_prev_actuator_sp = _actuator_sp;
-
-	// Allocate
-	_actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim);
-}
+static constexpr bool unused = validateSPIConfig(px4_spi_buses);

boards/ark/can-flow-mr/uavcan_board_identity

@@ -0,0 +1,17 @@
+# UAVCAN boot loadable Module ID
+set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
+set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
+add_definitions(
+	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
+	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
+)
+
+set(uavcanblid_hw_version_major 0)
+set(uavcanblid_hw_version_minor 88)
+set(uavcanblid_name "\"org.ark.can-flow-mr\"")
+
+add_definitions(
+	-DHW_UAVCAN_NAME=${uavcanblid_name}
+	-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
+	-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
+)

boards/ark/fmu-v6x/nuttx-config/nsh/defconfig

@@ -145,10 +145,11 @@ CONFIG_NETDB_DNSCLIENT=y
 CONFIG_NETDB_DNSCLIENT_ENTRIES=8
 CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
-CONFIG_NETINIT_DHCPC=y
+CONFIG_NETMAN_FALLBACK_IPADDR=0xC0A80004
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0xA290AFE
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_IPADDR=0xC0A80004
+CONFIG_NETINIT_DNSIPADDR=0xC0A800FE
+CONFIG_NETINIT_DRIPADDR=0xC0A80001
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49

boards/ark/fpv/default.px4board

@@ -16,9 +16,9 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
-CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -36,6 +36,7 @@ CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y

boards/bitcraze/crazyflie/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/bitcraze/crazyflie21/default.px4board

@@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/cuav/7-nano/src/board_config.h

@@ -352,6 +352,7 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT,	        \
 		GPIO_SAFETY_SWITCH_IN,			\
 		GPIO_nARMED,				\
+		GPIO_PWM_LEVEL_CONTROL,			\
 	}
 
 #define BOARD_ENABLE_CONSOLE_BUFFER

boards/diatone/mamba-f405-mk2/default.px4board

@@ -36,7 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

boards/flywoo/gn-f405/default.px4board

@@ -27,7 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

boards/holybro/kakutef7/default.px4board

@@ -41,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/matek/h743-slim/default.px4board

@@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
+CONFIG_BOARD_PARAM_FILE="/fs/microsd/params"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y

boards/micoair/h743-v2/default.px4board

@@ -26,10 +26,10 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TAP_ESC=y
-CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/micoair/h743-v2/nuttx-config/bootloader/defconfig

@@ -35,7 +35,6 @@ CONFIG_CDCACM_PRODUCTID=0x004b
 CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2"
 CONFIG_CDCACM_RXBUFSIZE=6000
 CONFIG_CDCACM_TXBUFSIZE=12000
-#TODO：ally for VENDOR ID in the future
 CONFIG_CDCACM_VENDORID=0x3162
 CONFIG_CDCACM_VENDORSTR="MicoAir"
 CONFIG_DEBUG_FULLOPT=y

boards/micoair/h743-v2/nuttx-config/nsh/defconfig

@@ -196,17 +196,17 @@ CONFIG_STM32H7_SDMMC1=y
 CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32H7_SPI2=y
-CONFIG_STM32H7_SPI3=y
 CONFIG_STM32H7_SPI2_DMA=y
 CONFIG_STM32H7_SPI2_DMA_BUFFER=2048
+CONFIG_STM32H7_SPI3=y
 CONFIG_STM32H7_SPI3_DMA=y
 CONFIG_STM32H7_SPI3_DMA_BUFFER=2048
 CONFIG_STM32H7_SPI_DMATHRESHOLD=8
+CONFIG_STM32H7_TIM15=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
 CONFIG_STM32H7_TIM8=y
-CONFIG_STM32H7_TIM15=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART5=y
 CONFIG_STM32H7_UART7=y
@@ -222,25 +222,18 @@ CONFIG_STM32H7_USART_SWAP=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_UART4_BAUD=115200
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=800
-CONFIG_UART5_BAUD=115200
 CONFIG_UART5_RXBUFSIZE=600
 CONFIG_UART5_TXBUFSIZE=800
-CONFIG_UART7_BAUD=115200
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_TXBUFSIZE=800
-CONFIG_UART8_BAUD=115200
 CONFIG_UART8_RXBUFSIZE=600
 CONFIG_UART8_TXBUFSIZE=800
-CONFIG_USART1_BAUD=115200
 CONFIG_USART1_RXBUFSIZE=1200
 CONFIG_USART1_TXBUFSIZE=1500
-CONFIG_USART2_BAUD=115200
 CONFIG_USART2_RXBUFSIZE=600
 CONFIG_USART2_TXBUFSIZE=800
-CONFIG_USART3_BAUD=115200
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=800
 CONFIG_USART6_BAUD=57600

boards/modalai/fc-v1/default.px4board

@@ -43,6 +43,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
@@ -104,4 +105,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
-# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set

boards/modalai/voxl2/target/voxl-px4

@@ -74,49 +74,49 @@ print_config_settings(){
 while getopts "bcdhfmorwz" flag
 do
     case "${flag}" in
-        b) 
+        b)
             echo "[INFO] Holybro GPS selected"
             GPS=HOLYBRO
             ;;
-        c) 
+        c)
             echo "[INFO] Wiping old config file"
             if [ -f "$CONFIG_FILE" ]; then
                 rm -rf ${CONFIG_FILE}
             fi
             exit 0
             ;;
-        d) 
+        d)
             echo "[INFO] Disabling daemon mode"
             DAEMON_MODE=DISABLE
             ;;
-        h) 
+        h)
             print_usage
             ;;
-        f) 
+        f)
             echo "[INFO] Setting RC to FAKE_RC_INPUT"
             RC=FAKE_RC_INPUT
             ;;
-        m) 
+        m)
             echo "[INFO] Matek GPS selected"
             GPS=MATEK
             ;;
-        o) 
+        o)
             echo "[INFO] OSD module selected"
             OSD=ENABLE
             ;;
-        r) 
+        r)
             echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
             RC=CRSF_MAV
             ;;
-        w) 
+        w)
             echo "[INFO] TBS Crossfire RC receiver, raw selected"
             RC=CRSF_RAW
             ;;
-        z) 
+        z)
             echo "[INFO] Fake sensor calibration values selected"
             SENSOR_CAL=FAKE
             ;;
-        *) 
+        *)
             print_usage
             ;;
     esac
@@ -130,9 +130,9 @@ else
 fi
 
 if [ $SENSOR_CAL == "FAKE" ]; then
-   /bin/echo "[INFO] Setting up fake sensor calibration values"
-   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
-   /bin/sync
+    /bin/echo "[INFO] Setting up fake sensor calibration values"
+    px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
+    /bin/sync
 fi
 
 print_config_settings

boards/modalai/voxl2/target/voxl-px4-hitl-start

@@ -35,9 +35,9 @@ fi
 /bin/sleep 1
 
 if [ ! -f /data/px4/param/hitl_parameters ]; then
-   echo "[INFO] Setting default parameters for PX4 on voxl"
+    echo "[INFO] Setting default parameters for PX4 on voxl"
     . /etc/modalai/voxl-px4-hitl-set-default-parameters.config
-   /bin/sync
+    /bin/sync
 else
     param select /data/px4/param/hitl_parameters
     param load

boards/modalai/voxl2/target/voxl-px4-start

@@ -13,7 +13,7 @@ echo "OSD: $OSD"
 echo "EXTRA STEPS:"
 for i in "${EXTRA_STEPS[@]}"
 do
-	echo -e "\t$i"
+    echo -e "\t$i"
 done
 echo -e "*************************\n"
 
@@ -83,17 +83,17 @@ qshell ist8310 start -R 10 -X -b 1
 
 # GPS and magnetometer
 if [ "$GPS" != "NONE" ]; then
-	# On M0052 the GPS driver runs on the apps processor
-	if [ $PLATFORM = "M0052" ]; then
-	    gps start -d /dev/ttyHS2
-	# On M0054 and M0104 the GPS driver runs on SLPI DSP
-	else
-	    qshell gps start -d 6
-	fi
+    # On M0052 the GPS driver runs on the apps processor
+    if [ $PLATFORM = "M0052" ]; then
+        gps start -d /dev/ttyHS2
+    # On M0054 and M0104 the GPS driver runs on SLPI DSP
+    else
+        qshell gps start -d 6
+    fi
 fi
 
 # Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will
-# fail but not cause any harm. 
+# fail but not cause any harm.
 /bin/echo "Looking for ncp5623c RGB LED"
 qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56
 
@@ -107,17 +107,17 @@ param touch SYS_AUTOSTART
 # ESC driver
 if [ "$ESC" == "VOXL_ESC" ]; then
     /bin/echo "Starting VOXL ESC driver"
-	qshell voxl_esc start
+    qshell voxl_esc start
 elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then
-	if [ "$RC" == "M0065_SBUS" ]; then
-		/bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
-		qshell voxl2_io start
-	else
-		/bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
-		qshell voxl2_io start -e
-	fi
+    if [ "$RC" == "M0065_SBUS" ]; then
+        /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC"
+        qshell voxl2_io start
+    else
+        /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC"
+        qshell voxl2_io start -e
+    fi
 else
-	/bin/echo "No ESC type specified, not starting an ESC driver"
+    /bin/echo "No ESC type specified, not starting an ESC driver"
 fi
 
 
@@ -133,41 +133,41 @@ elif [ "$RC" == "CRSF_MAV" ]; then
     qshell mavlink_rc_in start -m -p 7 -b 115200
 elif [ "$RC" == "SPEKTRUM" ]; then
     /bin/echo "Starting Spektrum RC"
-	# On M0052 the RC driver runs on the apps processor
-	if [ $PLATFORM = "M0052" ]; then
+    # On M0052 the RC driver runs on the apps processor
+    if [ $PLATFORM = "M0052" ]; then
         rc_input start -d /dev/ttyHS1
-	# On M0054 and M0104 the RC driver runs on SLPI DSP
-	else
+    # On M0054 and M0104 the RC driver runs on SLPI DSP
+    else
         qshell spektrum_rc start
-	fi
+    fi
 elif [ "$RC" == "GHST" ]; then
     /bin/echo "Starting GHST RC driver"
     qshell ghst_rc start -d 7
 elif [ "$RC" == "M0065_SBUS" ]; then
-	if [ $PLATFORM = "M0052" ]; then
-		apps_sbus start
-	elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
-    	/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
-    	qshell dsp_sbus start
-		retVal=$?
-		if [ $retVal -ne 0 ]; then
-	    	/bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
-	    	/bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
-			qshell voxl2_io start -d -p 7
-		fi
-	else
-    	/bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
-	fi
+    if [ $PLATFORM = "M0052" ]; then
+        apps_sbus start
+    elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
+        /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
+        qshell dsp_sbus start
+        retVal=$?
+        if [ $retVal -ne 0 ]; then
+            /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed"
+            /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW"
+            qshell voxl2_io start -d -p 7
+        fi
+    else
+        /bin/echo "M0065 SBUS RC driver already started with PWM ESC start"
+    fi
 fi
 
 if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then
-	# Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
-	qshell lightware_laser_serial start -d 7
+    # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor
+    qshell lightware_laser_serial start -d 7
 fi
 
 if [ "$POWER_MANAGER" == "VOXLPM" ]; then
-	# APM power monitor
-	qshell voxlpm start -X -b 2
+    # APM power monitor
+    qshell voxlpm start -X -b 2
 fi
 
 # Optional distance sensor on spare i2c
@@ -191,7 +191,7 @@ qshell load_mon start
 # is publishing input_rc topics. Otherwise for external RC
 # over Mavlink this isn't needed.
 if [ "$RC" != "EXTERNAL" ]; then
-	qshell rc_update start
+    qshell rc_update start
 fi
 
 qshell commander start
@@ -214,7 +214,7 @@ voxl_save_cal_params start
 # On M0052 there is only one IMU. So, PX4 needs to
 # publish IMU samples externally for VIO to use.
 if [ $PLATFORM = "M0052" ]; then
-	imu_server start
+    imu_server start
 fi
 
 # start the onboard fast link to connect to voxl-mavlink-server
@@ -250,5 +250,5 @@ fi
 # Start optional EXTRA_STEPS
 for i in "${EXTRA_STEPS[@]}"
 do
-	$i
+    $i
 done

boards/nxp/tropic-community/src/CMakeLists.txt

@@ -42,8 +42,12 @@ px4_add_library(drivers_board
 	imxrt_flexspi_nor_boot.c
 	imxrt_flexspi_nor_flash.c
 	imxrt_flexspi_storage.c
+	imxrt_ocram_initialize.c
 )
 
+# Force compiler not to use builtin functions (like memcpy)
+# to optimize for loops in init.c (imxrt_ocram_initialize)
+set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
 
 target_link_libraries(drivers_board
 	PRIVATE

boards/nxp/tropic-community/src/imxrt_ocram_initialize.c

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2019, 2023 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 imxrt_ocram_initialize.c
+ *
+ * PX4 fmu-v6xrt RAM startup early startup code.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "arm_internal.h"
+#include "imxrt_iomuxc.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+__BEGIN_DECLS
+extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
+extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
+extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
+extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
+extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
+__END_DECLS
+
+/****************************************************************************
+ * Name: imxrt_ocram_initialize
+ *
+ * Description:
+ *   Called off reset vector to reconfigure the flexRAM
+ *   and finish the FLASH to RAM Copy.
+ *   CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
+ *
+ ****************************************************************************/
+
+__EXPORT void imxrt_ocram_initialize(void)
+{
+	uint32_t regval;
+	register uint64_t *src;
+	register uint64_t *dest;
+
+	/* FlexRAM Configuration
+	 *    F = 64K ITCM
+	 *    A = 64K DTCM
+	 *    5 = 64K OCRAM
+	 *    So 0xFFFFFFAA is
+	 *    384K FlexRAM ITCM
+	 *    128K FlexRAM DTCM
+	 * */
+
+	putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
+	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
+	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
+
+	/* Copy any necessary code sections from FLASH to ITCM. The process is the
+	* same as the code copying from FLASH to RAM above. */
+	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
+	     dest < (uint64_t *)&_eitcmfuncs;) {
+		*dest++ = *src++;
+	}
+
+	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
+	* certain that there are no issues with the state of global variables.
+	*/
+
+	for (dest = &_sdtcm; dest < &_edtcm;) {
+		*dest++ = 0;
+	}
+}

boards/nxp/tropic-community/src/init.c

@@ -104,11 +104,6 @@ extern void led_off(int led);
 
 extern uint32_t _srodata;            /* Start of .rodata */
 extern uint32_t _erodata;            /* End of .rodata */
-extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
-extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
-extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
-extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
-extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
 __END_DECLS
 
 /************************************************************************************
@@ -192,50 +187,6 @@ void imxrt_flash_setup_prefetch_partition(void)
 	ARM_DMB();
 }
 
-/****************************************************************************
- * Name: imxrt_ocram_initialize
- *
- * Description:
- *   Called off reset vector to reconfigure the flexRAM
- *   and finish the FLASH to RAM Copy.
- *
- ****************************************************************************/
-
-__EXPORT void imxrt_ocram_initialize(void)
-{
-	uint32_t regval;
-	register uint64_t *src;
-	register uint64_t *dest;
-
-	/* FlexRAM Configuration
-	 *    F = 64K ITCM
-	 *    A = 64K DTCM
-	 *    5 = 64K OCRAM
-	 *    So 0xFFFFFFAA is
-	 *    384K FlexRAM ITCM
-	 *    128K FlexRAM DTCM
-	 * */
-
-	putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
-	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
-	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
-
-	/* Copy any necessary code sections from FLASH to ITCM. The process is the
-	* same as the code copying from FLASH to RAM above. */
-	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
-	     dest < (uint64_t *)&_eitcmfuncs;) {
-		*dest++ = *src++;
-	}
-
-	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
-	* certain that there are no issues with the state of global variables.
-	*/
-
-	for (dest = &_sdtcm; dest < &_edtcm;) {
-		*dest++ = 0;
-	}
-}
-
 /****************************************************************************
  * Name: imxrt_boardinitialize
  *

boards/omnibus/f4sd/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v2/default.px4board

@@ -41,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v6xrt/src/CMakeLists.txt

@@ -75,9 +75,14 @@ else()
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
 		imxrt_clockconfig.c
+		imxrt_ocram_initialize.c
 		${SRCS}
 	)
 
+	# Force compiler not to use builtin functions (like memcpy)
+	# to optimize for loops in init.c (imxrt_ocram_initialize)
+	set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
+
 	target_link_libraries(drivers_board
 		PRIVATE
 			arch_board_hw_info

boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2019, 2023 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 imxrt_ocram_initialize.c
+ *
+ * PX4 fmu-v6xrt RAM startup early startup code.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "arm_internal.h"
+#include "imxrt_iomuxc.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+__BEGIN_DECLS
+extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
+extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
+extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
+extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
+extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
+__END_DECLS
+
+/****************************************************************************
+ * Name: imxrt_ocram_initialize
+ *
+ * Description:
+ *   Called off reset vector to reconfigure the flexRAM
+ *   and finish the FLASH to RAM Copy.
+ *   CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
+ *
+ ****************************************************************************/
+
+__EXPORT void imxrt_ocram_initialize(void)
+{
+	uint32_t regval;
+	register uint64_t *src;
+	register uint64_t *dest;
+
+	/* Reallocate
+	 * Final Configuration is
+	 *    No DTCM
+	 *    512k  OCRAM M7 (FlexRAM)          (2038:0000-203f:ffff)
+	 *    128k  OCRAMM7 FlexRAM ECC         (2036:0000-2037:ffff)
+	 *    64k   OCRAM2 ECC parity           (2035:0000-2035:ffff)
+	 *    64k   OCRAM1 ECC parity           (2034:0000-2034:ffff)
+	 *    512k  FlexRAM OCRAM2              (202C:0000-2033:ffff)
+	 *    512k  FlexRAM OCRAM1              (2024:0000-202B:ffff)
+	 *    256k  System  OCRAM M4            (2020:0000-2023:ffff)
+	 */
+
+	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
+	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
+	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
+	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
+
+	/* Copy any necessary code sections from FLASH to ITCM. The process is the
+	* same as the code copying from FLASH to RAM above. */
+	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
+	     dest < (uint64_t *)&_eitcmfuncs;) {
+		*dest++ = *src++;
+	}
+
+	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
+	* certain that there are no issues with the state of global variables.
+	*/
+
+	for (dest = &_sdtcm; dest < &_edtcm;) {
+		*dest++ = 0;
+	}
+
+#if defined(CONFIG_BOOT_RUNFROMISRAM)
+	const uint32_t *src;
+	uint32_t *dest;
+
+	for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
+	     dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
+	     dest < (uint32_t *) &_etext;) {
+		*dest++ = *src++;
+	}
+
+#endif
+}

boards/px4/fmu-v6xrt/src/init.c

@@ -109,11 +109,6 @@ extern void led_off(int led);
 
 extern uint32_t _srodata;            /* Start of .rodata */
 extern uint32_t _erodata;            /* End of .rodata */
-extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
-extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
-extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
-extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
-extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
 __END_DECLS
 
 /************************************************************************************
@@ -225,65 +220,7 @@ void imxrt_flash_setup_prefetch_partition(void)
 	ARM_ISB();
 	ARM_DMB();
 }
-/****************************************************************************
- * Name: imxrt_ocram_initialize
- *
- * Description:
- *   Called off reset vector to reconfigure the flexRAM
- *   and finish the FLASH to RAM Copy.
- *
- ****************************************************************************/
 
-__EXPORT void imxrt_ocram_initialize(void)
-{
-	uint32_t regval;
-	register uint64_t *src;
-	register uint64_t *dest;
-
-	/* Reallocate
-	 * Final Configuration is
-	 *    No DTCM
-	 *    512k  OCRAM M7 (FlexRAM)          (2038:0000-203f:ffff)
-	*    128k  OCRAMM7 FlexRAM ECC         (2036:0000-2037:ffff)
-	*    64k   OCRAM2 ECC parity           (2035:0000-2035:ffff)
-	*    64k   OCRAM1 ECC parity           (2034:0000-2034:ffff)
-	*    512k  FlexRAM OCRAM2              (202C:0000-2033:ffff)
-	 *    512k  FlexRAM OCRAM1              (2024:0000-202B:ffff)
-	 *    256k  System  OCRAM M4            (2020:0000-2023:ffff)
-	 */
-
-	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
-	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
-	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
-	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
-
-	/* Copy any necessary code sections from FLASH to ITCM. The process is the
-	* same as the code copying from FLASH to RAM above. */
-	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
-	     dest < (uint64_t *)&_eitcmfuncs;) {
-		*dest++ = *src++;
-	}
-
-	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
-	* certain that there are no issues with the state of global variables.
-	*/
-
-	for (dest = &_sdtcm; dest < &_edtcm;) {
-		*dest++ = 0;
-	}
-
-#if defined(CONFIG_BOOT_RUNFROMISRAM)
-	const uint32_t *src;
-	uint32_t *dest;
-
-	for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
-	     dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
-	     dest < (uint32_t *) &_etext;) {
-		*dest++ = *src++;
-	}
-
-#endif
-}
 
 /****************************************************************************
  * Name: imxrt_boardinitialize

boards/px4/sitl/default.px4board

@@ -80,3 +80,4 @@ CONFIG_EXAMPLES_HELLO=y
 CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y
 CONFIG_EXAMPLES_PX4_SIMPLE_APP=y
 CONFIG_EXAMPLES_WORK_ITEM=y
+CONFIG_MODULES_SPACECRAFT=n

boards/px4/sitl/spacecraft.px4board

@@ -0,0 +1,17 @@
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_RATE_CONTROL=n
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_MC_ATT_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
+CONFIG_MODULES_MC_POS_CONTROL=n
+CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
+CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
+CONFIG_EKF2_AUX_GLOBAL_POSITION=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=n
+CONFIG_MODULES_SPACECRAFT=y

boards/raspberrypi/pico/default.px4board

@@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

cmake/metadata.cmake

@@ -53,6 +53,7 @@ file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml
 # avoid param duplicates
 list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/")
 list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/")
+list(FILTER yaml_config_files EXCLUDE REGEX ".*/spacecraft/")
 
 add_custom_target(metadata_parameters
 	COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs

msg/Ekf2Timestamps.msg

@@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times
 # difference of +-3.2s to the sensor_combined topic.
 
 int16 airspeed_timestamp_rel
+int16 airspeed_validated_timestamp_rel
 int16 distance_sensor_timestamp_rel
 int16 optical_flow_timestamp_rel
 int16 vehicle_air_data_timestamp_rel

msg/EstimatorStatusFlags.msg

@@ -47,6 +47,7 @@ bool cs_rng_terrain             # 39 - true if we are fusing range finder data f
 bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain
 bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
+bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/VehicleImu.msg

@@ -9,8 +9,8 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that
 float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
 float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
 
-uint16 delta_angle_dt           # integration period in microseconds
-uint16 delta_velocity_dt        # integration period in microseconds
+uint32 delta_angle_dt           # integration period in microseconds
+uint32 delta_velocity_dt        # integration period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2

msg/versioned/VehicleLocalPosition.msg

@@ -77,10 +77,12 @@ float32 evv				# Standard deviation of vertical 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)
-float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
-float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
+# set to INFINITY when limiting not required
+float32 vxy_max				# maximum horizontal speed (meters/sec)
+float32 vz_max				# maximum vertical speed (meters/sec)
+float32 hagl_min			# minimum height above ground level (meters)
+float32 hagl_max_z			# maximum height above ground level for z-control (meters)
+float32 hagl_max_xy			# maximum height above ground level for xy-control (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
 # TOPICS estimator_local_position

msg/versioned/VehicleStatus.msg

@@ -49,7 +49,7 @@ uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position cont
 uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
-uint8 NAVIGATION_STATE_FREE1 = 16
+uint8 NAVIGATION_STATE_CS_PREFLIGHT_CHECK = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow

platforms/nuttx/CMakeLists.txt

@@ -506,6 +506,11 @@ if(NOT NUTTX_DIR MATCHES "external")
 		message(STATUS "Found SVD: ${DEBUG_SVD_FILE_PATH}")
 	endif()
 
+	if(NOT CMAKE_GDB)
+		find_program(CMAKE_GDB gdb-multiarch)
+		message(STATUS "Found GDB: ${CMAKE_GDB}")
+	endif()
+
 	if(DEBUG_DEVICE)
 		configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/launch.json.in ${PX4_SOURCE_DIR}/.vscode/launch.json @ONLY)
 	endif()

src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
+		CollisionPrevention
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -33,19 +33,18 @@
 
 #include "lightware_sf45_serial.hpp"
 
-#include <inttypes.h>
 #include <fcntl.h>
+#include <float.h>
+#include <inttypes.h>
 #include <termios.h>
+
 #include <lib/crc/crc.h>
 #include <lib/mathlib/mathlib.h>
-
-#include <float.h>
+#include <matrix/matrix/math.hpp>
+#include <ObstacleMath.hpp>
 
 using namespace time_literals;
 
-/* Configuration Constants */
-
-
 SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -93,6 +92,11 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
+	// set the sensor orientation
+	const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+				    (_yaw_cfg));
+	_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
+
 	start();
 	return PX4_OK;
 }
@@ -136,8 +140,6 @@ int SF45LaserSerial::measure()
 
 int SF45LaserSerial::collect()
 {
-	float distance_m = -1.0f;
-
 	if (_sensor_state == STATE_UNINIT) {
 
 		perf_begin(_sample_perf);
@@ -196,8 +198,7 @@ int SF45LaserSerial::collect()
 		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
 
 		if (_crc_valid) {
-			sf45_process_replies(&distance_m);
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
+			sf45_process_replies();
 			perf_end(_sample_perf);
 			return PX4_OK;
 		}
@@ -592,68 +593,62 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	}
 }
 
-void SF45LaserSerial::sf45_process_replies(float *distance_m)
+void SF45LaserSerial::sf45_process_replies()
 {
+	const int16_t YAW_THRESHOLD  = 32000;
+	const int16_t YAW_ADJUSTMENT = 65535;
+
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
-			int16_t scaled_yaw = 0;
 
 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
-			if (raw_yaw > 32000) {
-				raw_yaw = raw_yaw - 65535;
+			if (raw_yaw > YAW_THRESHOLD) {
+				raw_yaw -= YAW_ADJUSTMENT;
 			}
 
-			// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
+			// Adjust yaw for downward facing sensor
 			if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
-				raw_yaw = raw_yaw * -1;
+				raw_yaw = -raw_yaw;
 			}
 
 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw_sensor_frame = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw_frd          = ObstacleMath::wrap_360(scaled_yaw_sensor_frame + _obstacle_distance.angle_offset);
+			float distance_m              = raw_distance * SF45_SCALE_FACTOR;
 
-			switch (_yaw_cfg) {
-			case ROTATION_FORWARD_FACING:
-				break;
-
-			case ROTATION_BACKWARD_FACING:
-				if (scaled_yaw > 180) {
-					scaled_yaw = scaled_yaw - 180;
-
-				} else {
-					scaled_yaw = scaled_yaw + 180; // rotation facing aft
-				}
-
-				break;
-
-			case ROTATION_RIGHT_FACING:
-				scaled_yaw = scaled_yaw + 90; // rotation facing right
-				break;
-
-			case ROTATION_LEFT_FACING:
-				scaled_yaw = scaled_yaw - 90; // rotation facing left
-				break;
-
-			default:
-				break;
-			}
-
-			// Convert to meters for the debug message
-			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			// Update the current bin distance
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
 
-			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+			// Find bin index for the current sensor yaw angle (in sensor frame)
+			const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame);
 
 			if (current_bin != _previous_bin) {
-				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
-					  (double)*distance_m);
+				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw_frd, current_bin,
+					  (double)distance_m);
+
+				if (_vehicle_attitude_sub.updated()) {
+					vehicle_attitude_s vehicle_attitude;
+
+					if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+						_vehicle_attitude = matrix::Quatf(vehicle_attitude.q);
+					}
+				}
+
+				// Scale distance with vehicle rotation
+				float current_bin_dist = static_cast<float>(_current_bin_dist);
+				float scaled_yaw_frd_rad = math::radians(static_cast<float>(scaled_yaw_frd));
+				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_frd_rad, _vehicle_attitude);
+				_current_bin_dist = static_cast<uint16_t>(current_bin_dist);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
 				}
 
 				hrt_abstime now = hrt_absolute_time();
+
+				_obstacle_distance.distances[current_bin] = _current_bin_dist;
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -691,54 +686,23 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 {
 	// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
 	// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
-	uint8_t start;
-	uint8_t end;
 
-	if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
-		// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
-		// THis is simplyfied as we are not considering the scaning direction
-		start = math::max(previous_bin, current_bin);
-		end = math::min(previous_bin, current_bin);
+	// Shift bin indices such that we can never have the wrap-around case.
+	const float    fov_offset_angle    = 360.0f - SF45_FIELDOF_VIEW / 2.f;
+	const uint16_t current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  _obstacle_distance.increment,
+					     fov_offset_angle);
+	const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
+					     fov_offset_angle);
 
-	} else if (previous_bin < current_bin) {	// Scanning clockwise
-		start = previous_bin + 1;
-		end = current_bin;
+	const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	const uint16_t end   = math::max(current_bin_offset, previous_bin_offset);
 
-	} else { 					// scanning counter-clockwise
-		start = current_bin;
-		end = previous_bin - 1;
+	// populate the missed bins with the measurement
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle);
+		_obstacle_distance.distances[bin_index] = measurement;
+		_data_timestamps[bin_index] = now;
 	}
-
-	if (start <= end) {
-		for (uint8_t i = start; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-	} else { // wrap-around case
-		for (uint8_t i = start; i < BIN_COUNT; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-		for (uint8_t i = 0; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-	}
-}
-uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
-{
-	uint8_t mapped_sector = 0;
-	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
-
-	return mapped_sector;
-}
-
-float SF45LaserSerial::sf45_wrap_360(float f)
-{
-	return matrix::wrap(f, 0.f, 360.f);
 }
 
 uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -49,7 +49,9 @@
 #include <lib/perf/perf_counter.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "sf45_commands.h"
 
@@ -72,10 +74,6 @@ enum SF45_PARSED_STATE {
 };
 
 enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE
-	ROTATION_RIGHT_FACING = 2,    // MAV_SENSOR_ROTATION_YAW_90
-	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
-	ROTATION_LEFT_FACING = 6,     // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_UPWARD_FACING = 24,  // MAV_SENSOR_ROTATION_PITCH_90
 	ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
 };
@@ -92,9 +90,7 @@ public:
 	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void				sf45_process_replies(float *data);
-	uint8_t				sf45_convert_angle(const int16_t yaw);
-	float				sf45_wrap_360(float f);
+	void				sf45_process_replies();
 
 private:
 	obstacle_distance_s 			_obstacle_distance{};
@@ -103,6 +99,7 @@ private:
 				obstacle_distance_s::distances[0]);
 	static constexpr uint64_t 	SF45_MEAS_TIMEOUT{100_ms};
 	static constexpr float 		SF45_SCALE_FACTOR = 0.01f;
+	static constexpr float		SF45_FIELDOF_VIEW = 320.f; // degrees
 
 	void				start();
 	void				stop();
@@ -113,6 +110,7 @@ private:
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
 	void 				_publish_obstacle_msg(hrt_abstime now);
+	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];
 
 
@@ -141,6 +139,7 @@ private:
 	int32_t				_orient_cfg{0};
 	uint8_t				_previous_bin{0};
 	uint16_t			_current_bin_dist{UINT16_MAX};
+	matrix::Quatf			_vehicle_attitude{};
 
 	// end of SF45/B data members
 

src/drivers/drv_dshot.h

@@ -88,7 +88,7 @@ typedef enum {
  * @param channel_mask		Bitmask of channels (LSB = channel 0) to enable.
  *				This allows some of the channels to remain configured
  *				as GPIOs or as another function. Already used channels/timers will not be configured as DShot
- * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
+ * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600
  * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);

src/drivers/dshot/DShot.cpp

@@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			} else if (tim_config == -3) {
 				dshot_frequency_request = DSHOT600;
 
-			} else if (tim_config == -2) {
-				dshot_frequency_request = DSHOT1200;
-
 			} else {
 				_output_mask &= ~channels; // don't use for dshot
 			}
@@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output.
 It skips all pins already in use (e.g. by a camera trigger module).
 
 It supports:
-- DShot150, DShot300, DShot600, DShot1200
+- DShot150, DShot300, DShot600
 - telemetry via separate UART and publishing as esc_status message
 - sending DShot commands via CLI
 

src/drivers/dshot/DShot.h

@@ -52,7 +52,6 @@ using namespace time_literals;
 static constexpr unsigned int DSHOT150  =  150000u;
 static constexpr unsigned int DSHOT300  =  300000u;
 static constexpr unsigned int DSHOT600  =  600000u;
-static constexpr unsigned int DSHOT1200 = 1200000u;
 
 static constexpr int DSHOT_DISARM_VALUE = 0;
 static constexpr int DSHOT_MIN_THROTTLE = 1;
@@ -107,7 +106,6 @@ private:
 		DShot150  = 150,
 		DShot300  = 300,
 		DShot600  = 600,
-		DShot1200 = 1200,
 	};
 
 	struct Command {

src/drivers/ins/vectornav/VectorNav.cpp

@@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.vxy_max = INFINITY;
 			local_position.vz_max = INFINITY;
 			local_position.hagl_min = INFINITY;
-			local_position.hagl_max = INFINITY;
+			local_position.hagl_max_z = INFINITY;
+			local_position.hagl_max_xy = INFINITY;
 
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();

src/drivers/power_monitor/ina238/ina238.cpp

@@ -160,6 +160,8 @@ int INA238::Reset()
 
 	int ret = PX4_ERROR;
 
+	_retries = 3;
+
 	if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) {
 		return ret;
 	}
@@ -231,6 +233,16 @@ int INA238::collect()
 	success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
 	success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
 
+	if (success) {
+		_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
+		_battery.updateCurrent(static_cast<float>(current * _current_lsb));
+		_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
+
+		_battery.setConnected(success);
+
+		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+	}
+
 	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])) {
@@ -242,26 +254,21 @@ int INA238::collect()
 			PX4_DEBUG("register check failed");
 			perf_count(_bad_register_perf);
 			success = false;
+
+			_battery.setConnected(success);
+
+			_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 		}
 	}
 
-	if (!success) {
-		PX4_DEBUG("error reading from sensor");
-		bus_voltage = current = 0;
-	}
-
-	_battery.setConnected(success);
-	_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
-	_battery.updateCurrent(static_cast<float>(current * _current_lsb));
-	_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
-	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-
 	perf_end(_sample_perf);
 
 	if (success) {
 		return PX4_OK;
 
 	} else {
+		PX4_DEBUG("error reading from sensor");
+
 		return PX4_ERROR;
 	}
 }

src/drivers/pwm_out/module.yaml

@@ -23,7 +23,6 @@ actuator_output:
             -5: DShot150
             -4: DShot300
             -3: DShot600
-            -2: DShot1200
             -1: OneShot
             50: PWM 50 Hz
             100: PWM 100 Hz

src/drivers/uavcan/uavcan_main.cpp

@@ -1073,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run()
 void
 UavcanNode::print_info()
 {
-	(void)pthread_mutex_lock(&_node_mutex);
-
 	// Memory status
 	printf("Pool allocator status:\n");
 	printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n",
@@ -1112,15 +1110,29 @@ UavcanNode::print_info()
 	printf("\n");
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-	printf("ESC outputs:\n");
-	_mixing_interface_esc.mixingOutput().printStatus();
 
-	printf("Servo outputs:\n");
-	_mixing_interface_servo.mixingOutput().printStatus();
+	// Print esc status if at least one channel is enabled
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) {
+			printf("ESC outputs:\n");
+			_mixing_interface_esc.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
+	// Print servo status if at least one channel is enabled
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) {
+			printf("Servo outputs:\n");
+			_mixing_interface_servo.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
 #endif
 
-	printf("\n");
-
 	// Sensor bridges
 	for (const auto &br : _sensor_bridges) {
 		printf("Sensor '%s':\n", br->get_name());
@@ -1140,8 +1152,6 @@ UavcanNode::print_info()
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-
-	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 void

src/lib/CMakeLists.txt

@@ -31,7 +31,6 @@
 #
 ############################################################################
 
-add_subdirectory(actuator_effectiveness EXCLUDE_FROM_ALL)
 add_subdirectory(adsb EXCLUDE_FROM_ALL)
 add_subdirectory(airspeed EXCLUDE_FROM_ALL)
 add_subdirectory(atmosphere EXCLUDE_FROM_ALL)

src/lib/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "ActuatorEffectiveness.hpp"
-
-#include <px4_platform_common/log.h>
-
-int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque,
-		const matrix::Vector3f &thrust)
-{
-	int actuator_idx = num_actuators_matrix[selected_matrix];
-
-	if (actuator_idx >= NUM_ACTUATORS) {
-		PX4_ERR("Too many actuators");
-		return -1;
-	}
-
-	if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) {
-		PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)");
-		return -1;
-	}
-
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
-	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
-	matrix_selection_indexes[totalNumActuators()] = selected_matrix;
-	++num_actuators[(int)type];
-	return num_actuators_matrix[selected_matrix]++;
-}
-
-void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count)
-{
-	int total_count = totalNumActuators();
-
-	for (int i = 0; i < count; ++i) {
-		matrix_selection_indexes[i + total_count] = selected_matrix;
-	}
-
-	num_actuators[(int)type] += count;
-	num_actuators_matrix[selected_matrix] += count;
-}
-
-int ActuatorEffectiveness::Configuration::totalNumActuators() const
-{
-	int total_count = 0;
-
-	for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
-		total_count += num_actuators_matrix[i];
-	}
-
-	return total_count;
-}
-
-void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp)
-{
-	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
-		const uint32_t motor_mask = (1u << actuator_idx);
-
-		if (stoppable_motors_mask & motor_mask) {
-
-			// Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy)
-			if (fabsf(actuator_sp(actuator_idx)) < .02f) {
-				_stopped_motors_mask |= motor_mask;
-
-			} else {
-				_stopped_motors_mask &= ~motor_mask;
-			}
-		}
-	}
-}

src/lib/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -1,226 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectiveness.hpp
- *
- * Interface for Actuator Effectiveness
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#pragma once
-
-#include <cstdint>
-
-#include <matrix/matrix/math.hpp>
-#include <uORB/topics/control_allocator_status.h>
-
-enum class AllocationMethod {
-	NONE = -1,
-	PSEUDO_INVERSE = 0,
-	SEQUENTIAL_DESATURATION = 1,
-	AUTO = 2,
-	METRIC = 3,
-};
-
-enum class ActuatorType {
-	MOTORS = 0,
-	SERVOS,
-	THRUSTERS,
-	COUNT
-};
-
-enum class EffectivenessUpdateReason {
-	NO_EXTERNAL_UPDATE = 0,
-	CONFIGURATION_UPDATE = 1,
-	MOTOR_ACTIVATION_UPDATE = 2,
-};
-
-
-class ActuatorEffectiveness
-{
-public:
-	ActuatorEffectiveness() = default;
-	virtual ~ActuatorEffectiveness() = default;
-
-	static constexpr int NUM_ACTUATORS = 16;
-	static constexpr int NUM_AXES = 6;
-
-	enum ControlAxis {
-		ROLL = 0,
-		PITCH,
-		YAW,
-		THRUST_X,
-		THRUST_Y,
-		THRUST_Z
-	};
-
-	static constexpr int MAX_NUM_MATRICES = 2;
-
-	using EffectivenessMatrix = matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS>;
-	using ActuatorVector = matrix::Vector<float, NUM_ACTUATORS>;
-
-	enum class FlightPhase {
-		HOVER_FLIGHT = 0,
-		FORWARD_FLIGHT = 1,
-		TRANSITION_HF_TO_FF = 2,
-		TRANSITION_FF_TO_HF = 3
-	};
-
-	struct Configuration {
-		/**
-		 * Add an actuator to the selected matrix, returning the index, or -1 on error
-		 */
-		int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust);
-
-		/**
-		 * Call this after manually adding N actuators to the selected matrix
-		 */
-		void actuatorsAdded(ActuatorType type, int count);
-
-		int totalNumActuators() const;
-
-		/// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos
-		EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES];
-
-		int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices
-		ActuatorVector trim[MAX_NUM_MATRICES];
-
-		ActuatorVector linearization_point[MAX_NUM_MATRICES];
-
-		int selected_matrix;
-
-		uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES];
-		int num_actuators[(int)ActuatorType::COUNT];
-	};
-
-	/**
-	 * Set the current flight phase
-	 *
-	 * @param Flight phase
-	 */
-	virtual void setFlightPhase(const FlightPhase &flight_phase)
-	{
-		_flight_phase = flight_phase;
-	}
-
-	/**
-	 * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES.
-	 * This is expected to stay constant.
-	 */
-	virtual int numMatrices() const { return 1; }
-
-	/**
-	 * Get the desired allocation method(s) for each matrix, if configured as AUTO
-	 */
-	virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const
-	{
-		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
-			allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE;
-		}
-	}
-
-	/**
-	 * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized
-	 */
-	virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const
-	{
-		for (int i = 0; i < MAX_NUM_MATRICES; ++i) {
-			normalize[i] = false;
-		}
-	}
-
-	/**
-	 * Get the control effectiveness matrix if updated
-	 *
-	 * @return true if updated and matrix is set
-	 */
-	virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;}
-
-	/**
-	 * Get the current flight phase
-	 *
-	 * @return Flight phase
-	 */
-	const FlightPhase &getFlightPhase() const
-	{
-		return _flight_phase;
-	}
-
-	/**
-	 * Display name
-	 */
-	virtual const char *name() const = 0;
-
-	/**
-	 * Callback from the control allocation, allowing to manipulate the setpoint.
-	 * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers).
-	 *
-	 * @param actuator_sp input & output setpoint
-	 */
-	virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {}
-
-	/**
-	 * Callback from the control allocation, allowing to manipulate the setpoint.
-	 * This can be used to e.g. add non-linear or external terms.
-	 * It is called after the matrix multiplication and before final clipping.
-	 * @param actuator_sp input & output setpoint
-	 */
-	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
-
-	/**
-	 * Get a bitmask of motors to be stopped
-	 */
-	virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; }
-
-	/**
-	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
-	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
-	 */
-	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
-
-	/**
-	 * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero
-	 *
-	 * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand
-	 * @param actuator_sp outcome of the allocation to determine if the motor should be stopped
-	 */
-	virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp);
-
-protected:
-	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};
-	uint32_t _stopped_motors_mask{0};
-};

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.cpp

@@ -1,312 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessRotors.hpp
- *
- * Actuator effectiveness computed from rotors position and orientation
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#include "ActuatorEffectivenessRotors.hpp"
-
-// #include "ActuatorEffectivenessTilts.hpp"
-
-using namespace matrix;
-
-ActuatorEffectivenessRotors::ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config,
-		bool tilt_support)
-	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
-{
-	for (int i = 0; i < NUM_ROTORS_MAX; ++i) {
-		char buffer[17];
-		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PX", i);
-		_param_handles[i].position_x = param_find(buffer);
-		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PY", i);
-		_param_handles[i].position_y = param_find(buffer);
-		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_PZ", i);
-		_param_handles[i].position_z = param_find(buffer);
-
-		if (_axis_config == AxisConfiguration::Configurable) {
-			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AX", i);
-			_param_handles[i].axis_x = param_find(buffer);
-			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AY", i);
-			_param_handles[i].axis_y = param_find(buffer);
-			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_AZ", i);
-			_param_handles[i].axis_z = param_find(buffer);
-		}
-
-		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_CT", i);
-		_param_handles[i].thrust_coef = param_find(buffer);
-
-		snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_KM", i);
-		_param_handles[i].moment_ratio = param_find(buffer);
-
-		if (_tilt_support) {
-			snprintf(buffer, sizeof(buffer), "CA_ROTOR%u_TILT", i);
-			_param_handles[i].tilt_index = param_find(buffer);
-		}
-	}
-
-	updateParams();
-}
-
-void ActuatorEffectivenessRotors::updateParams()
-{
-	ModuleParams::updateParams();
-
-	_geometry.num_rotors = math::min(NUM_ROTORS_MAX, static_cast<int>(_param_ca_rotor_count.get()));
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		Vector3f &position = _geometry.rotors[i].position;
-		param_get(_param_handles[i].position_x, &position(0));
-		param_get(_param_handles[i].position_y, &position(1));
-		param_get(_param_handles[i].position_z, &position(2));
-
-		Vector3f &axis = _geometry.rotors[i].axis;
-
-		switch (_axis_config) {
-		case AxisConfiguration::Configurable:
-			param_get(_param_handles[i].axis_x, &axis(0));
-			param_get(_param_handles[i].axis_y, &axis(1));
-			param_get(_param_handles[i].axis_z, &axis(2));
-			break;
-
-		case AxisConfiguration::FixedForward:
-			axis = Vector3f(1.f, 0.f, 0.f);
-			break;
-
-		case AxisConfiguration::FixedUpwards:
-			axis = Vector3f(0.f, 0.f, -1.f);
-			break;
-		}
-
-		param_get(_param_handles[i].thrust_coef, &_geometry.rotors[i].thrust_coef);
-		param_get(_param_handles[i].moment_ratio, &_geometry.rotors[i].moment_ratio);
-
-		if (_tilt_support) {
-			int32_t tilt_param{0};
-			param_get(_param_handles[i].tilt_index, &tilt_param);
-			_geometry.rotors[i].tilt_index = tilt_param - 1;
-
-		} else {
-			_geometry.rotors[i].tilt_index = -1;
-		}
-	}
-}
-
-bool
-ActuatorEffectivenessRotors::addActuators(Configuration &configuration)
-{
-	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
-		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
-		return false;
-	}
-
-	int num_actuators = computeEffectivenessMatrix(_geometry,
-			    configuration.effectiveness_matrices[configuration.selected_matrix],
-			    configuration.num_actuators_matrix[configuration.selected_matrix]);
-	configuration.actuatorsAdded(ActuatorType::MOTORS, num_actuators);
-	return true;
-}
-
-int
-ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry,
-		EffectivenessMatrix &effectiveness, int actuator_start_index)
-{
-	int num_actuators = 0;
-
-	for (int i = 0; i < geometry.num_rotors; i++) {
-
-		if (i + actuator_start_index >= NUM_ACTUATORS) {
-			break;
-		}
-
-		++num_actuators;
-
-		// Get rotor axis
-		Vector3f axis = geometry.rotors[i].axis;
-
-		// Normalize axis
-		float axis_norm = axis.norm();
-
-		if (axis_norm > FLT_EPSILON) {
-			axis /= axis_norm;
-
-		} else {
-			// Bad axis definition, ignore this rotor
-			continue;
-		}
-
-		// Get rotor position
-		const Vector3f &position = geometry.rotors[i].position;
-
-		// Get coefficients
-		float ct = geometry.rotors[i].thrust_coef;
-		float km = geometry.rotors[i].moment_ratio;
-
-		if (geometry.propeller_torque_disabled) {
-			km = 0.f;
-		}
-
-		if (geometry.propeller_torque_disabled_non_upwards) {
-			bool upwards = fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f;
-
-			if (!upwards) {
-				km = 0.f;
-			}
-		}
-
-		if (fabsf(ct) < FLT_EPSILON) {
-			continue;
-		}
-
-		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
-
-		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis;
-
-		// Fill corresponding items in effectiveness matrix
-		for (size_t j = 0; j < 3; j++) {
-			effectiveness(j, i + actuator_start_index) = moment(j);
-			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
-		}
-
-		if (geometry.yaw_by_differential_thrust_disabled) {
-			// set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts)
-			effectiveness(2, i + actuator_start_index) = 0.f;
-		}
-
-		if (geometry.three_dimensional_thrust_disabled) {
-			// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
-			// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
-			// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
-			// by the allocator directly.
-
-			effectiveness(0 + 3, i + actuator_start_index) = 0.f;
-			effectiveness(1 + 3, i + actuator_start_index) = 0.f;
-			effectiveness(2 + 3, i + actuator_start_index) = -ct;
-		}
-	}
-
-	return num_actuators;
-}
-
-// TODO(@Jaeyoung-Lim): This should not be in the Rotors effectiveness, so we need to move it elsewhere
-/*
-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
-		float collective_tilt_control)
-{
-	if (!PX4_ISFINITE(collective_tilt_control)) {
-		collective_tilt_control = -1.f;
-	}
-
-	uint32_t nontilted_motors = 0;
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		int tilt_index = _geometry.rotors[i].tilt_index;
-
-		if (tilt_index == -1 || tilt_index >= tilts.count()) {
-			nontilted_motors |= 1u << i;
-			continue;
-		}
-
-		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
-		const float tilt_direction = math::radians((float)tilt.tilt_direction);
-		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
-	}
-
-	return nontilted_motors;
-}
-*/
-
-Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_direction)
-{
-	Vector3f axis{0.f, 0.f, -1.f};
-	return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis;
-}
-
-uint32_t ActuatorEffectivenessRotors::getMotors() const
-{
-	uint32_t motors = 0;
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		motors |= 1u << i;
-	}
-
-	return motors;
-}
-
-uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const
-{
-	uint32_t upwards_motors = 0;
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		const Vector3f &axis = _geometry.rotors[i].axis;
-
-		if (fabsf(axis(0)) < 0.1f && fabsf(axis(1)) < 0.1f && axis(2) < -0.5f) {
-			upwards_motors |= 1u << i;
-		}
-	}
-
-	return upwards_motors;
-}
-
-uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const
-{
-	uint32_t forward_motors = 0;
-
-	for (int i = 0; i < _geometry.num_rotors; ++i) {
-		const Vector3f &axis = _geometry.rotors[i].axis;
-
-		if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) {
-			forward_motors |= 1u << i;
-		}
-	}
-
-	return forward_motors;
-}
-
-bool
-ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration,
-		EffectivenessUpdateReason external_update)
-{
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
-		return false;
-	}
-
-	return addActuators(configuration);
-}

src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp

@@ -1,156 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessRotors.hpp
- *
- * Actuator effectiveness computed from rotors position and orientation
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#pragma once
-
-#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
-
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-
-class ActuatorEffectivenessTilts;
-
-using namespace time_literals;
-
-class ActuatorEffectivenessRotors : public ModuleParams, public ActuatorEffectiveness
-{
-public:
-	enum class AxisConfiguration {
-		Configurable, ///< axis can be configured
-		FixedForward, ///< axis is fixed, pointing forwards (positive X)
-		FixedUpwards, ///< axis is fixed, pointing upwards (negative Z)
-	};
-
-	static constexpr int NUM_ROTORS_MAX = 12;
-
-	struct RotorGeometry {
-		matrix::Vector3f position;
-		matrix::Vector3f axis;
-		float thrust_coef;
-		float moment_ratio;
-		int tilt_index;
-	};
-
-	struct Geometry {
-		RotorGeometry rotors[NUM_ROTORS_MAX];
-		int num_rotors{0};
-		bool propeller_torque_disabled{false};
-		bool yaw_by_differential_thrust_disabled{false};
-		bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
-		bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt
-	};
-
-	ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
-				    bool tilt_support = false);
-	virtual ~ActuatorEffectivenessRotors() = default;
-
-	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
-
-	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
-	{
-		allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION;
-	}
-
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
-	{
-		normalize[0] = true;
-	}
-
-	static int computeEffectivenessMatrix(const Geometry &geometry,
-					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
-
-	bool addActuators(Configuration &configuration);
-
-	const char *name() const override { return "Rotors"; }
-
-	/**
-	 * Sets the motor axis from tilt configurations and current tilt control.
-	 * @param tilts configured tilt servos
-	 * @param tilt_control current tilt control in [-1, 1] (can be NAN)
-	 * @return the motors as bitset which are not tiltable
-	 */
-	uint32_t updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control);
-
-	const Geometry &geometry() const { return _geometry; }
-
-	/**
-	 * Get the tilted axis {0, 0, -1} rotated by -tilt_angle around y, then
-	 * rotated by tilt_direction around z.
-	 */
-	static matrix::Vector3f tiltedAxis(float tilt_angle, float tilt_direction);
-
-	void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; }
-
-	void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; }
-
-	void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
-
-	void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
-
-	uint32_t getMotors() const;
-	uint32_t getUpwardsMotors() const;
-	uint32_t getForwardsMotors() const;
-
-private:
-	void updateParams() override;
-	const AxisConfiguration _axis_config;
-	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
-
-	struct ParamHandles {
-		param_t position_x;
-		param_t position_y;
-		param_t position_z;
-		param_t axis_x;
-		param_t axis_y;
-		param_t axis_z;
-		param_t thrust_coef;
-		param_t moment_ratio;
-		param_t tilt_index;
-	};
-	ParamHandles _param_handles[NUM_ROTORS_MAX];
-
-	Geometry _geometry{};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_ROTOR_COUNT>) _param_ca_rotor_count
-	)
-};

src/lib/actuator_effectiveness/ActuatorEffectivenessRotorsTest.cpp

@@ -1,135 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessRotors.cpp
- *
- * Tests for Control Allocation Algorithms
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#include <gtest/gtest.h>
-#include "ActuatorEffectivenessRotors.hpp"
-
-using namespace matrix;
-
-TEST(ActuatorEffectivenessRotors, AllZeroCase)
-{
-	// Quad wide geometry
-	ActuatorEffectivenessRotors::Geometry geometry = {};
-	geometry.rotors[0].position(0) = 1.0f;
-	geometry.rotors[0].position(1) = 1.0f;
-	geometry.rotors[0].position(2) = 0.0f;
-	geometry.rotors[0].axis(0) = 0.0f;
-	geometry.rotors[0].axis(1) = 0.0f;
-	geometry.rotors[0].axis(2) = -1.0f;
-	geometry.rotors[0].thrust_coef = 1.0f;
-	geometry.rotors[0].moment_ratio = 0.05f;
-
-	geometry.rotors[1].position(0) = -1.0f;
-	geometry.rotors[1].position(1) = -1.0f;
-	geometry.rotors[1].position(2) = 0.0f;
-	geometry.rotors[1].axis(0) = 0.0f;
-	geometry.rotors[1].axis(1) = 0.0f;
-	geometry.rotors[1].axis(2) = -1.0f;
-	geometry.rotors[1].thrust_coef = 1.0f;
-	geometry.rotors[1].moment_ratio = 0.05f;
-
-	geometry.rotors[2].position(0) = 1.0f;
-	geometry.rotors[2].position(1) = -1.0f;
-	geometry.rotors[2].position(2) = 0.0f;
-	geometry.rotors[2].axis(0) = 0.0f;
-	geometry.rotors[2].axis(1) = 0.0f;
-	geometry.rotors[2].axis(2) = -1.0f;
-	geometry.rotors[2].thrust_coef = 1.0f;
-	geometry.rotors[2].moment_ratio = -0.05f;
-
-	geometry.rotors[3].position(0) = -1.0f;
-	geometry.rotors[3].position(1) = 1.0f;
-	geometry.rotors[3].position(2) = 0.0f;
-	geometry.rotors[3].axis(0) = 0.0f;
-	geometry.rotors[3].axis(1) = 0.0f;
-	geometry.rotors[3].axis(2) = -1.0f;
-	geometry.rotors[3].thrust_coef = 1.0f;
-	geometry.rotors[3].moment_ratio = -0.05f;
-
-	geometry.num_rotors = 4;
-
-	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
-	effectiveness.setZero();
-	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
-
-	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
-		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-	};
-	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
-
-	EXPECT_EQ(effectiveness, effectiveness_expected);
-}
-
-TEST(ActuatorEffectivenessRotors, Tilt)
-{
-	Vector3f axis_expected{0.f, 0.f, -1.f};
-	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{1.f, 0.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{-1.f, 0.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, 0.f, -1.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, 1.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, -1.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.cpp

@@ -1,205 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessThrusters.hpp
- *
- * Actuator effectiveness computed from thrusters position and orientation
- *
- * @author Pedro Roque, <padr@kth.se>
- */
-
-#include "ActuatorEffectivenessThrusters.hpp"
-
-using namespace matrix;
-
-ActuatorEffectivenessThrusters::ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config,
-		bool tilt_support)
-	: ModuleParams(parent), _axis_config(axis_config), _tilt_support(tilt_support)
-{
-
-	for (int i = 0; i < NUM_THRUSTERS_MAX; ++i) {
-		char buffer[17];
-		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PX", i);
-		_param_handles[i].position_x = param_find(buffer);
-		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PY", i);
-		_param_handles[i].position_y = param_find(buffer);
-		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_PZ", i);
-		_param_handles[i].position_z = param_find(buffer);
-
-		if (_axis_config == AxisConfiguration::Configurable) {
-			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AX", i);
-			_param_handles[i].axis_x = param_find(buffer);
-			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AY", i);
-			_param_handles[i].axis_y = param_find(buffer);
-			snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_AZ", i);
-			_param_handles[i].axis_z = param_find(buffer);
-		}
-
-		snprintf(buffer, sizeof(buffer), "CA_THRUSTER%u_CT", i);
-		_param_handles[i].thrust_coef = param_find(buffer);
-
-		if (_tilt_support) {
-			PX4_ERR("Tilt support not implemented");
-		}
-	}
-
-	_count_handle = param_find("CA_THRUSTER_CNT");
-
-	updateParams();
-}
-
-void ActuatorEffectivenessThrusters::updateParams()
-{
-	ModuleParams::updateParams();
-
-	int32_t count = 0;
-
-	if (param_get(_count_handle, &count) != 0) {
-		PX4_ERR("param_get failed");
-		return;
-	}
-
-	_geometry.num_thrusters = math::min(NUM_THRUSTERS_MAX, (int)count);
-
-	for (int i = 0; i < _geometry.num_thrusters; ++i) {
-		Vector3f &position = _geometry.thrusters[i].position;
-		param_get(_param_handles[i].position_x, &position(0));
-		param_get(_param_handles[i].position_y, &position(1));
-		param_get(_param_handles[i].position_z, &position(2));
-
-		Vector3f &axis = _geometry.thrusters[i].axis;
-
-		switch (_axis_config) {
-		case AxisConfiguration::Configurable:
-			param_get(_param_handles[i].axis_x, &axis(0));
-			param_get(_param_handles[i].axis_y, &axis(1));
-			param_get(_param_handles[i].axis_z, &axis(2));
-			break;
-		}
-
-		param_get(_param_handles[i].thrust_coef, &_geometry.thrusters[i].thrust_coef);
-	}
-}
-
-bool
-ActuatorEffectivenessThrusters::addActuators(Configuration &configuration)
-{
-	if (configuration.num_actuators[(int)ActuatorType::SERVOS] > 0) {
-		PX4_ERR("Wrong actuator ordering: servos need to be after motors");
-		return false;
-	}
-
-	int num_actuators = computeEffectivenessMatrix(_geometry,
-			    configuration.effectiveness_matrices[configuration.selected_matrix],
-			    configuration.num_actuators_matrix[configuration.selected_matrix]);
-	configuration.actuatorsAdded(ActuatorType::THRUSTERS, num_actuators);
-	return true;
-}
-
-int
-ActuatorEffectivenessThrusters::computeEffectivenessMatrix(const Geometry &geometry,
-		EffectivenessMatrix &effectiveness, int actuator_start_index)
-{
-	int num_actuators = 0;
-
-	for (int i = 0; i < geometry.num_thrusters; i++) {
-
-		if (i + actuator_start_index >= NUM_ACTUATORS) {
-			break;
-		}
-
-		++num_actuators;
-
-		// Get rotor axis
-		Vector3f axis = geometry.thrusters[i].axis;
-
-		// Normalize axis
-		float axis_norm = axis.norm();
-
-		if (axis_norm > FLT_EPSILON) {
-			axis /= axis_norm;
-
-		} else {
-			// Bad axis definition, ignore this rotor
-			continue;
-		}
-
-		// Get rotor position
-		const Vector3f &position = geometry.thrusters[i].position;
-
-		// Get coefficients
-		float ct = geometry.thrusters[i].thrust_coef;
-
-		if (fabsf(ct) < FLT_EPSILON) {
-			continue;
-		}
-
-		// Compute thrust generated by this rotor
-		matrix::Vector3f thrust = ct * axis;
-
-		// Compute moment generated by this rotor
-		matrix::Vector3f moment = ct * position.cross(axis);
-
-		// Fill corresponding items in effectiveness matrix
-		for (size_t j = 0; j < 3; j++) {
-			effectiveness(j, i + actuator_start_index) = moment(j);
-			effectiveness(j + 3, i + actuator_start_index) = thrust(j);
-		}
-	}
-
-	return num_actuators;
-}
-
-
-uint32_t ActuatorEffectivenessThrusters::getThrusters() const
-{
-	uint32_t thrusters = 0;
-
-	for (int i = 0; i < _geometry.num_thrusters; ++i) {
-		thrusters |= 1u << i;
-	}
-
-	return thrusters;
-}
-
-bool
-ActuatorEffectivenessThrusters::getEffectivenessMatrix(Configuration &configuration,
-		EffectivenessUpdateReason external_update)
-{
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
-		return false;
-	}
-
-	return addActuators(configuration);
-}

src/lib/actuator_effectiveness/ActuatorEffectivenessThrusters.hpp

@@ -1,121 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessThrusters.hpp
- *
- * Actuator effectiveness computed from thrusters position and orientation
- *
- * @author Pedro Roque, <padr@kth.se>
- */
-
-#pragma once
-
-#include "ActuatorEffectiveness.hpp"
-
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-
-class ActuatorEffectivenessTilts;
-
-using namespace time_literals;
-
-class ActuatorEffectivenessThrusters : public ModuleParams, public ActuatorEffectiveness
-{
-public:
-	enum class AxisConfiguration {
-		Configurable, ///< axis can be configured
-	};
-
-	static constexpr int NUM_THRUSTERS_MAX = 12;
-
-	struct ThrusterGeometry {
-		matrix::Vector3f position;
-		matrix::Vector3f axis;
-		float thrust_coef;
-	};
-
-	struct Geometry {
-		ThrusterGeometry thrusters[NUM_THRUSTERS_MAX];
-		int num_thrusters{0};
-	};
-
-	ActuatorEffectivenessThrusters(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
-				       bool tilt_support = false);
-	virtual ~ActuatorEffectivenessThrusters() = default;
-
-	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
-
-	void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override
-	{
-		allocation_method_out[0] =
-			AllocationMethod::SEQUENTIAL_DESATURATION;  // TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
-	}
-
-	void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override
-	{
-		// TODO(@Jaeyoung-Lim): needs to be updated based on metric mixer
-		normalize[0] = true;
-	}
-
-	static int computeEffectivenessMatrix(const Geometry &geometry,
-					      EffectivenessMatrix &effectiveness, int actuator_start_index = 0);
-
-	bool addActuators(Configuration &configuration);
-
-	const char *name() const override { return "Thrusters"; }
-
-	const Geometry &geometry() const { return _geometry; }
-
-	uint32_t getThrusters() const;
-
-private:
-	void updateParams() override;
-	const AxisConfiguration _axis_config;
-	const bool _tilt_support; ///< if true, tilt servo assignment params are loaded
-
-	struct ParamHandles {
-		param_t position_x;
-		param_t position_y;
-		param_t position_z;
-		param_t axis_x;
-		param_t axis_y;
-		param_t axis_z;
-		param_t thrust_coef;
-	};
-	ParamHandles _param_handles[NUM_THRUSTERS_MAX];
-	param_t _count_handle;
-
-	Geometry _geometry{};
-};

src/lib/actuator_effectiveness/ActuatorEffectivenessThrustersTest.cpp

@@ -1,135 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * 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 ActuatorEffectivenessRotors.cpp
- *
- * Tests for Control Allocation Algorithms
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-#include <gtest/gtest.h>
-#include "ActuatorEffectivenessRotors.hpp"
-
-using namespace matrix;
-
-TEST(ActuatorEffectivenessRotors, AllZeroCase)
-{
-	// Quad wide geometry
-	ActuatorEffectivenessRotors::Geometry geometry = {};
-	geometry.rotors[0].position(0) = 1.0f;
-	geometry.rotors[0].position(1) = 1.0f;
-	geometry.rotors[0].position(2) = 0.0f;
-	geometry.rotors[0].axis(0) = 0.0f;
-	geometry.rotors[0].axis(1) = 0.0f;
-	geometry.rotors[0].axis(2) = -1.0f;
-	geometry.rotors[0].thrust_coef = 1.0f;
-	geometry.rotors[0].moment_ratio = 0.05f;
-
-	geometry.rotors[1].position(0) = -1.0f;
-	geometry.rotors[1].position(1) = -1.0f;
-	geometry.rotors[1].position(2) = 0.0f;
-	geometry.rotors[1].axis(0) = 0.0f;
-	geometry.rotors[1].axis(1) = 0.0f;
-	geometry.rotors[1].axis(2) = -1.0f;
-	geometry.rotors[1].thrust_coef = 1.0f;
-	geometry.rotors[1].moment_ratio = 0.05f;
-
-	geometry.rotors[2].position(0) = 1.0f;
-	geometry.rotors[2].position(1) = -1.0f;
-	geometry.rotors[2].position(2) = 0.0f;
-	geometry.rotors[2].axis(0) = 0.0f;
-	geometry.rotors[2].axis(1) = 0.0f;
-	geometry.rotors[2].axis(2) = -1.0f;
-	geometry.rotors[2].thrust_coef = 1.0f;
-	geometry.rotors[2].moment_ratio = -0.05f;
-
-	geometry.rotors[3].position(0) = -1.0f;
-	geometry.rotors[3].position(1) = 1.0f;
-	geometry.rotors[3].position(2) = 0.0f;
-	geometry.rotors[3].axis(0) = 0.0f;
-	geometry.rotors[3].axis(1) = 0.0f;
-	geometry.rotors[3].axis(2) = -1.0f;
-	geometry.rotors[3].thrust_coef = 1.0f;
-	geometry.rotors[3].moment_ratio = -0.05f;
-
-	geometry.num_rotors = 4;
-
-	ActuatorEffectiveness::EffectivenessMatrix effectiveness;
-	effectiveness.setZero();
-	ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness);
-
-	const float expected[ActuatorEffectiveness::NUM_AXES][ActuatorEffectiveness::NUM_ACTUATORS] = {
-		{-1.0f,   1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 1.0f,  -1.0f,   1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.05f,  0.05f, -0.05f, -0.05f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{ 0.f,    0.f,    0.f,    0.f,   0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
-		{-1.0f,  -1.0f,  -1.0f,  -1.0f,  0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
-	};
-	ActuatorEffectiveness::EffectivenessMatrix effectiveness_expected(expected);
-
-	EXPECT_EQ(effectiveness, effectiveness_expected);
-}
-
-TEST(ActuatorEffectivenessRotors, Tilt)
-{
-	Vector3f axis_expected{0.f, 0.f, -1.f};
-	Vector3f axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{1.f, 0.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{1.f / sqrtf(2.f), 0.f, -1.f / sqrtf(2.f)};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{-1.f, 0.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, 0.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, 0.f, -1.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(0.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, 1.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(M_PI_F / 2.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-
-	axis_expected = Vector3f{0.f, -1.f, 0.f};
-	axis = ActuatorEffectivenessRotors::tiltedAxis(-M_PI_F / 2.f, M_PI_F / 2.f);
-	EXPECT_EQ(axis, axis_expected);
-}

src/lib/collision_prevention/CMakeLists.txt

@@ -31,7 +31,13 @@
 #
 ############################################################################
 
-px4_add_library(CollisionPrevention CollisionPrevention.cpp)
+px4_add_library(CollisionPrevention
+	CollisionPrevention.cpp
+	ObstacleMath.cpp
+)
 target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
+target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(CollisionPrevention PRIVATE mathlib)
 
 px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention)
+px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention)

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 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
@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include "ObstacleMath.hpp"
 #include <px4_platform_common/events.h>
 
 using namespace matrix;
@@ -235,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (convert to world frame and shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
-								  obstacle.increment / 2.f);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
-								  obstacle.increment / 2.f);
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment,
+							obstacle.angle_offset - vehicle_orientation_deg);
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment,
+							obstacle.angle_offset - vehicle_orientation_deg);
 
 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -276,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; j < 360 / obstacle.increment; j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
+							_obstacle_map_body_frame.angle_offset);
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset);
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset);
 
 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -372,7 +373,7 @@ void
 CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
 {
 	const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
-	const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
+	const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) -
 					       _obstacle_map_body_frame.angle_offset);
 	_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
 	// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
@@ -393,31 +394,22 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 
 	// discard values below min range
 	if (distance_reading > distance_sensor.min_distance) {
-		float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
+		float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+					    (distance_sensor.orientation), distance_sensor.q);
 		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
 
 		// calculate the field of view boundary bin indices
 		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
-
-		const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
-
-		const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
-
-		const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
-
-		const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
-
 		if (distance_reading < distance_sensor.max_distance) {
-			distance_reading = distance_reading * sensor_dist_scale;
+			ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude);
 		}
 
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm
 
 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
-			int wrapped_bin = _wrap_bin(bin);
+			int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT);
 
 			if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
 				_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
@@ -444,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 		float mean_dist = 0;
 
 		for (int j = i - filter_size; j <= i + filter_size; j++) {
-			int bin = _wrap_bin(j);
+			int bin = ObstacleMath::wrap_bin(j, BIN_COUNT);
 
 			if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
 				mean_dist += _param_cp_dist.get() * 100.f;
@@ -454,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 			}
 		}
 
-		const int bin = _wrap_bin(i);
+		const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT);
 		mean_dist = mean_dist / (2.f * filter_size + 1.f);
 		const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
 		const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
@@ -474,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 	}
 }
 
-float
-CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
-{
-	float offset = math::max(math::radians(angle_offset), 0.f);
-
-	switch (distance_sensor.orientation) {
-	case distance_sensor_s::ROTATION_YAW_0:
-		offset = 0.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_45:
-		offset = M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_90:
-		offset = M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_135:
-		offset = 3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_180:
-		offset = M_PI_F;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_225:
-		offset = -3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_270:
-		offset = -M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_315:
-		offset = -M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_CUSTOM:
-		offset = Eulerf(Quatf(distance_sensor.q)).psi();
-		break;
-	}
-
-	return offset;
-}
-
 float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 {
 	float obstacle_distance = 0.f;
@@ -529,10 +475,10 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 		Vector2f dir = direction / direction_norm;
 		const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
 		const float sp_angle_with_offset_deg =
-			_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
-		int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
-		dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
-		obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
+			ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
+
+		const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg);
+		obstacle_distance   = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
 	}
 
 	return obstacle_distance;
@@ -641,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter()
 	command.timestamp = getTime();
 	_vehicle_command_pub.publish(command);
 }
-
-float CollisionPrevention::_wrap_360(const float f)
-{
-	return wrap(f, 0.f, 360.f);
-}
-
-int CollisionPrevention::_wrap_bin(int i)
-{
-	i = i % BIN_COUNT;
-
-	while (i < 0) {
-		i += BIN_COUNT;
-	}
-
-	return i;
-}

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 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
@@ -204,13 +204,6 @@ private:
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual   /**< maximum velocity in manual flight mode*/
 	)
 
-	/**
-	 * Transforms the sensor orientation into a yaw in the local frame
-	 * @param distance_sensor, distance sensor message
-	 * @param angle_offset, sensor body frame offset
-	 */
-	float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
-
 	/**
 	 * Computes collision free setpoints
 	 * @param setpoint, setpoint before collision prevention intervention
@@ -238,6 +231,4 @@ private:
 	 */
 	void _publishVehicleCmdDoLoiter();
 
-	static float _wrap_360(const float f);
-	static int _wrap_bin(int i);
 };

src/lib/collision_prevention/ObstacleMath.cpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "ObstacleMath.hpp"
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+namespace ObstacleMath
+{
+
+void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle)
+{
+	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
+	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
+	const Vector3f forward(1.f, 0.f, 0.f);
+	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
+
+	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
+	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
+	distance *= horizontal_projection_scale;
+}
+
+int get_bin_at_angle(float bin_width, float angle)
+{
+	int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
+	return wrap_bin(bin_at_angle, 360 / bin_width);
+}
+
+float get_lower_bound_angle(int bin, float bin_width, float angle_offset)
+{
+	bin = wrap_bin(bin, 360 / bin_width);
+	return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f);
+}
+
+int get_offset_bin_index(int bin, float bin_width, float angle_offset)
+{
+	int offset = get_bin_at_angle(bin_width, angle_offset);
+	return wrap_bin(bin - offset, 360 / bin_width);
+}
+
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4])
+{
+	float offset = 0.0f;
+
+	switch (orientation) {
+	case SensorOrientation::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_CUSTOM:
+		if (q != nullptr) {
+			offset = Eulerf(Quatf(q)).psi();
+		}
+
+		break;
+	}
+
+	return offset;
+}
+
+int wrap_bin(int bin, int bin_count)
+{
+	return (bin + bin_count) % bin_count;
+}
+
+float wrap_360(const float angle)
+{
+	return matrix::wrap(angle, 0.0f, 360.0f);
+}
+
+} // ObstacleMath