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.
preflight check: fix range
2026-06-04 16:20:05 +08:00 · 2025-02-18 13:03:45 +01:00 · 2025-02-18 13:03:38 +01:00 · 2025-02-18 11:08:12 +01:00 · 2025-02-18 11:08:06 +01:00 · 2025-02-17 17:46:39 +01:00
47 changed files with 1415 additions and 280 deletions
@@ -129,5 +129,4 @@
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
        },
-        "ros.distro": "humble"
 }
@@ -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
@@ -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
@@ -114,5 +114,8 @@ px4_add_romfs_files(
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

+	71001_gazebo-classic_spacecraft_dart
+	71002_gz_spacecraft_2d
+
 	# [22000, 22999] Reserve for custom models
 )
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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():
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -595,35 +595,37 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8

 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));

 			// 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}"
-			float 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;

-			// Adjust for sensor orientation
-			scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
-
-			// Convert to meters for the debug message
-			float 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 %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
+				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()) {
@@ -634,9 +636,10 @@ void SF45LaserSerial::sf45_process_replies()
 					}
 				}

+				// Scale distance with vehicle rotation
 				float current_bin_dist = static_cast<float>(_current_bin_dist);
-				float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
-				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
+				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) {
@@ -702,21 +705,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 	}
 }

-
-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 = floor(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)
 {
 	uint32_t i;
@@ -74,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
 };
@@ -95,8 +91,6 @@ public:
 	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();
-	uint8_t				sf45_convert_angle(const int16_t yaw);
-	float				sf45_wrap_360(float f);

 private:
 	obstacle_distance_s 			_obstacle_distance{};
@@ -236,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) {
@@ -277,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) {
@@ -373,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
@@ -394,7 +394,8 @@ 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
@@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		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);
@@ -435,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;
@@ -445,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];
@@ -465,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;
@@ -520,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;
@@ -632,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;
-}
@@ -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);
 };
@@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle)
 	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)
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4])
 {
 	float offset = 0.0f;

@@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
 	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;
@@ -109,4 +122,9 @@ 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
@@ -45,6 +45,7 @@ enum SensorOrientation {
 	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
 	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+	ROTATION_CUSTOM  = 100,	  // MAV_SENSOR_ROTATION_CUSTOM

 	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
 	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
@@ -56,7 +57,7 @@ enum SensorOrientation {
 * Converts a sensor orientation to a yaw offset
 * @param orientation sensor orientation
 */
-float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr);

 /**
 * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
@@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
 */
 int get_bin_at_angle(float bin_width, float angle);

+/**
+ * Returns lower bound angle of a bin
+ * @param bin bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+float get_lower_bound_angle(int bin, float bin_width, float angle_offset);
+
 /**
 * Returns bin index for the current bin after an angle offset
 * @param bin current bin index
@@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset);
 */
 int wrap_bin(int bin, int bin_count);

+/**
+ * Wraps an angle to the range [0, 360)
+ * @param angle angle in degrees
+ */
+float wrap_360(const float angle);
+
+
 } // ObstacleMath
@@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	EXPECT_EQ(bin_index, 18);
 }

+TEST(ObstacleMathTest, GetLowerBound)
+{
+	// GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset
+	int      bin          = -1;
+	float    bin_width    =  7.5f;
+	float    angle_offset = -4.3f;
+
+	// WHEN: we calculate the lower bound angle of the bin
+	float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset);
+
+	// THEN: the lower bound angle should be correct. The bin index is wrapped to the end and
+	// the angle offset is applied in the counter-clockwise direction.
+	EXPECT_FLOAT_EQ(lower_bound, 344.45);
+
+}
+

 TEST(ObstacleMathTest, OffsetBinIndex)
 {
@@ -1572,6 +1572,42 @@ void Commander::handleCommandsFromModeExecutors()
 	}
 }

+unsigned Commander::handleCommandControlTest(const vehicle_command_s &cmd)
+{
+
+	// TODO: trigger this some different way using params for dev purposes...?
+	// TODO: decode from command: do we want to test roll/pitch/yaw(/collective tilt), or
+	// individual control surfaces?
+	// TODO define these somewhere else like all the cool kids do
+	static const int TEST_MODE_INDIVIDUAL = 0;
+	static const int TEST_MODE_RPY = 1;
+
+	int test_mode = TEST_MODE_RPY;
+
+	if (test_mode == TEST_MODE_INDIVIDUAL) {
+		// _user_mode_intention.change()
+		// _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_SERVO_TEST);
+		PX4_INFO("Not implemented");
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+
+	} else if (test_mode == TEST_MODE_RPY) {
+
+		// this nice pattern stolen from handle_command
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+	} else {
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	}
+
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+}
+
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@@ -1835,6 +1871,34 @@ void Commander::run()
 			_status_changed = true;
 		}

+		if (_param_com_do_cs_check.get()) {
+
+			// directly modify user intention here.
+			// plan is for this to ultimately to be triggered by a mavlink command
+			// through Commander::handle_command
+
+			// this nice pattern stolen from handle_command
+			// if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK, ModeChangeSource::ModeExecutor, false)) {
+			_prev_nav_state = _vehicle_status.nav_state;
+			_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+
+			// no error handling like this for now
+			// if (ret) {
+				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				// PX4_INFO("mode intention changed");
+
+			// } else {
+				// printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				// PX4_INFO("mode intention not changed");
+			// }
+
+		} else {
+			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+				_user_mode_intention.change(_prev_nav_state);
+			}
+		}
+
 		modeManagementUpdate();

 		const hrt_abstime now = hrt_absolute_time();
@@ -157,6 +157,8 @@ private:

 	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);

+	unsigned handleCommandControlTest(const vehicle_command_s &cmd);
+
 	void executeActionRequest(const action_request_s &action_request);

 	void printRejectMode(uint8_t nav_state);
@@ -284,6 +286,8 @@ private:

 	vehicle_land_detected_s	_vehicle_land_detected{};

+	uint8_t _prev_nav_state;
+
 	// commander publications
 	actuator_armed_s        _actuator_armed{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
@@ -349,6 +353,7 @@ private:
 		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
+		(ParamBool<px4::params::COM_DO_CS_CHECK>)   _param_com_do_cs_check,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
 	)
 };
@@ -590,66 +590,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			}
 		}
 	}
-
-	const hrt_abstime now = hrt_absolute_time();
-
-	/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
-	* for a short time interval after takeoff.
-	* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
-	* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
-	* if this does not fix the issue we need to stop using a position controlled
-	* mode to prevent flyaway crashes.
-	*/
-
-	if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-
-		if (!context.isArmed()) {
-			_nav_test_failed = false;
-			_nav_test_passed = false;
-
-		} else {
-			if (!_nav_test_passed) {
-				// Both test ratios need to pass/fail together to change the nav test status
-				const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
-							     && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
-
-				const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
-
-				if (innovation_pass) {
-					_time_last_innov_pass = now;
-
-					// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
-					const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s);
-					const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
-
-					// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
-					if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) {
-						_nav_test_passed = true;
-						_nav_test_failed = false;
-					}
-
-				} else if (innovation_fail) {
-					_time_last_innov_fail = now;
-
-					if (now > _time_last_innov_pass + 2_s) {
-						// if the innovation test has failed continuously, declare the nav as failed
-						_nav_test_failed = true;
-						/* EVENT
-						 * @description
-						 * Land and recalibrate the sensors.
-						 */
-						reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate,
-								       events::ID("check_estimator_nav_failure"),
-								       events::Log::Emergency, "Navigation failure");
-
-						if (reporter.mavlink_log_pub()) {
-							mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
-						}
-					}
-				}
-			}
-		}
-	}
 }

 void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
@@ -739,8 +679,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
 	// Check if quality checking of position accuracy and consistency is to be performed
 	const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get();

-	bool xy_valid = lpos.xy_valid && !_nav_test_failed;
-	bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
+	bool xy_valid = lpos.xy_valid;
+	bool v_xy_valid = lpos.v_xy_valid;

 	if (!context.isArmed()) {
 		if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) {
@@ -100,12 +100,6 @@ private:
 	hrt_abstime	_last_lpos_relaxed_fail_time_us{0};	///< Last time that the relaxed local position validity recovery check failed (usec)
 	hrt_abstime	_last_lvel_fail_time_us{0};	///< Last time that the local velocity validity recovery check failed (usec)

-	// variables used to check for navigation failure after takeoff
-	hrt_abstime	_time_last_innov_pass{0};	///< last time velocity and position innovations passed
-	hrt_abstime	_time_last_innov_fail{0};	///< last time velocity and position innovations failed
-	bool		_nav_test_passed{false};	///< true if the post takeoff navigation test has passed
-	bool		_nav_test_failed{false};	///< true if the post takeoff navigation test has failed
-
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};

@@ -178,6 +178,21 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_allocation_enabled = true;
 		break;

+	// enabling literally all of these makes the allocator run nicely.
+	// enabling only allocator, it will not run.
+	// surely there is like one or two which we really need and the rest can be dropped...
+	case vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK:
+		vehicle_control_mode.flag_control_auto_enabled = true;
+		vehicle_control_mode.flag_control_position_enabled = true;
+		vehicle_control_mode.flag_control_velocity_enabled = true;
+		vehicle_control_mode.flag_control_altitude_enabled = true;
+		vehicle_control_mode.flag_control_climb_rate_enabled = true;
+		vehicle_control_mode.flag_control_attitude_enabled = true;
+		vehicle_control_mode.flag_control_rates_enabled = true;
+		vehicle_control_mode.flag_control_allocation_enabled = true;
+		break;
+
+
 	// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
 	default:
 		break;
@@ -1029,3 +1029,6 @@ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
 *
 */
 PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);
+
+// temporary, to test control surface preflight check.
+PARAM_DEFINE_INT32(COM_DO_CS_CHECK, 0);
@@ -61,6 +61,8 @@ ControlAllocator::ControlAllocator() :
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();

+	_tiltrotor_extra_controls_pub.advertise();
+
 	for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
 		char buffer[17];
 		snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);
@@ -270,6 +272,14 @@ ControlAllocator::update_effectiveness_source()
 			tmp = new ActuatorEffectivenessHelicopterCoaxial(this);
 			break;

+		case EffectivenessSource::SPACECRAFT_2D:
+			// spacecraft_allocation does allocation and publishes directly to actuator_motors topic
+			break;
+
+		case EffectivenessSource::SPACECRAFT_3D:
+			// spacecraft_allocation does allocation and publishes directly to actuator_motors topic
+			break;
+
 		default:
 			PX4_ERR("Unknown airframe");
 			break;
@@ -338,6 +348,8 @@ ControlAllocator::Run()

 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;

+			_preflight_check_running = vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;
+
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};

 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -429,8 +441,19 @@ ControlAllocator::Run()
 			}
 		}

+		if (_preflight_check_running) {
+			preflight_check_overwrite_torque_sp(c);
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {

+			ActuatorEffectivenessTiltrotorVTOL *casted = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL *>
+					(_actuator_effectiveness);
+
+			if (casted != nullptr) {
+				casted->_preflight_check_running = _preflight_check_running;
+			}
+
 			_control_allocation[i]->setControlSetpoint(c[i]);

 			// Do allocation
@@ -465,6 +488,82 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }

+// void ControlAllocator::test_individual_control_surfaces() {
+	// goal here: modify actuation at the servo level.
+
+	// in here: small state machine cycling through servos (or taking info
+	// from outside about which servo to actuate)
+	// if test running: if enough time passed: go to next thing
+	//                  if last thing: test = not running
+
+	// elsewhere (probably Run()...)
+	// set test running if right message received
+	// if test running,
+
+// }
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
+
+	// cycle through roll, pitch, yaw, and for each one inject positive and
+	// negative torque setpoints.
+
+	// is this the proper way to do it?
+	// bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+	bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+
+	int n_axes = 3;
+	if (tiltrotor) {
+		n_axes = 4;
+	}
+
+	int max_phase = 2 * n_axes;
+
+	hrt_abstime now = hrt_absolute_time();
+	if (now - _last_preflight_check_update >= 500_ms) {
+		_preflight_check_phase++;
+		_preflight_check_phase %= max_phase;  // or quit once we did the whole thing once?
+		_last_preflight_check_update = now;
+	}
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	float modified_tilt_control = 0.5f;
+
+	if (axis < 3) {
+		c[0](0) = 0.;
+		c[0](1) = 0.;
+		c[0](2) = 0.;
+		c[0](axis) = negative ? -1.f : 1.f;
+
+		if (_num_control_allocation > 1) {
+			c[1](0) = 0.;
+			c[1](1) = 0.;
+			c[1](2) = 0.;
+			c[1](axis) = negative ? -1.f : 1.f;
+		}
+
+	} else {
+		// axis 4 = tiltrotor.
+		// collective tilt normalised control goes from 0 to 1.
+		modified_tilt_control = negative ? 0.f : 1.f;
+	}
+
+	tiltrotor_extra_controls_s tiltrotor_extra_controls;
+
+	if (!_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+		// got no message, make up thrust setpoint
+		tiltrotor_extra_controls.collective_thrust_normalized_setpoint = 0.;
+	}
+
+	tiltrotor_extra_controls.collective_tilt_normalized_setpoint = modified_tilt_control;
+	tiltrotor_extra_controls.timestamp = hrt_absolute_time();
+	_tiltrotor_extra_controls_pub.publish(tiltrotor_extra_controls);
+
+	// PX4_INFO("_torque_sp: %f, %f, %f", (double) _torque_sp(0), (double) _torque_sp(1), (double) _torque_sp(2));
+
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {
@@ -138,6 +138,8 @@ private:

 	void publish_actuator_controls();

+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -158,6 +160,8 @@ private:
 		HELICOPTER_TAIL_ESC = 10,
 		HELICOPTER_TAIL_SERVO = 11,
 		HELICOPTER_COAXIAL = 12,
+		SPACECRAFT_2D = 13,
+		SPACECRAFT_3D = 14,
 	};

 	enum class FailureMode {
@@ -177,6 +181,7 @@ private:

 	uORB::Subscription _vehicle_torque_setpoint1_sub{ORB_ID(vehicle_torque_setpoint), 1};  /**< vehicle torque setpoint subscription (2. instance) */
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
+	uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)};

 	// Outputs
 	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
@@ -185,6 +190,8 @@ private:
 	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
 	uORB::Publication<actuator_servos_trim_s>	_actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)};

+	uORB::Publication<tiltrotor_extra_controls_s>	_tiltrotor_extra_controls_pub{ORB_ID(tiltrotor_extra_controls)};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -199,6 +206,10 @@ private:
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};

+	bool _preflight_check_running{false};
+	int _preflight_check_phase{0};
+	hrt_abstime _last_preflight_check_update{0};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */

 	bool _armed{false};
@@ -166,7 +166,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {

 					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
-					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
+					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || _preflight_check_running) {
 						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;

 					} else {
@@ -88,6 +88,8 @@ public:

 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;

+	bool _preflight_check_running{false};
+
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
@@ -31,6 +31,8 @@ parameters:
                11: Helicopter (tail Servo)
                12: Helicopter (Coaxial)
                13: Rover (Mecanum)
+                14: Spacecraft 2D
+                15: Spacecraft 3D
            default: 0

        CA_METHOD:
@@ -374,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)

 	// Save current setpoints for the next FlightTask
 	trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
-	ekf_reset_counters_s last_reset_counters{};

 	if (isAnyTaskActive()) {
 		last_setpoint = _current_task.task->getTrajectorySetpoint();
-		last_reset_counters = _current_task.task->getResetCounters();
 	}

 	if (_initTask(new_task_index)) {
@@ -399,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::ActivationFailed;
 	}

-	_current_task.task->setResetCounters(last_reset_counters);
 	_command_failed = false;

 	return FlightTaskError::NoError;
@@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
 	_resetSetpoints();
 	_setDefaultConstraints();
+	initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
 	_gear = empty_landing_gear_default_keep;
 	return true;
@@ -24,6 +25,16 @@ void FlightTask::reActivate()
 	activate(setpoint_preserve_vertical);
 }

+void FlightTask::initEkfResetCounters()
+{
+	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
+}
+
 bool FlightTask::updateInitialize()
 {
 	_time_stamp_current = hrt_absolute_time();
@@ -55,15 +55,6 @@
 #include <uORB/topics/home_position.h>
 #include <lib/geo/geo.h>

-struct ekf_reset_counters_s {
-	uint8_t xy;
-	uint8_t vxy;
-	uint8_t z;
-	uint8_t vz;
-	uint8_t heading;
-	uint8_t hagl;
-};
-
 class FlightTask : public ModuleParams
 {
 public:
@@ -88,6 +79,8 @@ public:
 	 */
 	virtual void reActivate();

+	virtual void initEkfResetCounters();
+
 	/**
 	 * To be called to adopt parameters from an arrived vehicle command
 	 * @param command received command message containing the parameters
@@ -115,9 +108,6 @@ public:
 	 */
 	const trajectory_setpoint_s getTrajectorySetpoint();

-	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
-	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
-
 	/**
 	 * Get vehicle constraints.
 	 * The constraints can vary with task.
@@ -236,7 +226,14 @@ protected:
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};

-	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
+	struct ekf_reset_counters_s {
+		uint8_t xy;
+		uint8_t vxy;
+		uint8_t z;
+		uint8_t vz;
+		uint8_t heading;
+		uint8_t hagl;
+	} _reset_counters{}; ///< Counters for estimator local position resets

 	/**
 	 * Vehicle constraints.
@@ -200,7 +200,7 @@ void VehicleAirData::Run()
 					}

 					if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index
-					    && estimator_status_flags.cs_baro_fault) {
+					    && estimator_status_flags.cs_baro_fault && !_last_status_baro_fault) {
 						_priority[uorb_index] = 1; // 1 is min priority while still being enabled
 					}

@@ -225,6 +225,10 @@ void VehicleAirData::Run()
 		}
 	}

+	if (estimator_status_flags_updated) {
+		_last_status_baro_fault = estimator_status_flags.cs_baro_fault;
+	}
+
 	// check for the current best sensor
 	int best_index = 0;
 	_voter.get_best(time_now_us, &best_index);
@@ -126,6 +126,8 @@ private:

 	float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature

+	bool _last_status_baro_fault{false};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_BARO_QNH>) _param_sens_baro_qnh,
 		(ParamFloat<px4::params::SENS_BARO_RATE>) _param_sens_baro_rate
@@ -0,0 +1,51 @@
+############################################################################
+#
+#   Copyright (c) 2019 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_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_module(
+	MODULE modules__spacecraft
+	MAIN spacecraft
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	STACK_MAIN
+		3000
+	SRCS
+		SpacecraftHandler.cpp
+		SpacecraftHandler.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		mathlib
+		px4_work_queue
+)
@@ -0,0 +1,12 @@
+menuconfig MODULES_SPACECRAFT
+	bool "SPACECRAFT"
+	default n
+	---help---
+		Enable support for spacecraft
+
+menuconfig USER_SPACECRAFT
+	bool "spacecraft running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_SPACECRAFT
+	---help---
+		Put SPACECRAFT in userspace memory
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 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 SpacecraftHandler.cpp
+ *
+ * Control allocator.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#include "SpacecraftHandler.hpp"
+
+int SpacecraftHandler::task_spawn(int argc, char *argv[])
+{
+	return 0;
+}
+
+int SpacecraftHandler::print_status()
+{
+	PX4_INFO("Running");
+
+	return 0;
+}
+
+int SpacecraftHandler::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SpacecraftHandler::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+	### Description
+	This implements control allocation for spacecraft vehicles.
+	It takes torque and thrust setpoints as inputs and outputs
+	actuator setpoint messages.
+	)DESCR_STR"
+	);
+
+	PRINT_MODULE_USAGE_NAME("spacecraft", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+/**
+ * Control Allocator app start / stop handling function
+ */
+extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]);
+
+int spacecraft_main(int argc, char *argv[])
+{
+	return SpacecraftHandler::main(argc, argv);
+}
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 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 ControlAllocator.hpp
+ *
+ * Control allocator.
+ *
+ * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/actuator_servos.h>
+#include <uORB/topics/actuator_servos_trim.h>
+#include <uORB/topics/control_allocator_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/failure_detector_status.h>
+
+class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	SpacecraftHandler();
+
+	virtual ~SpacecraftHandler();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:		/**< loop duration performance counter */
+
+};
@@ -0,0 +1,235 @@
+__max_num_mc_motors: &max_num_mc_motors 12
+__max_num_thrusters: &max_num_thrusters 12
+__max_num_servos: &max_num_servos 8
+__max_num_tilts: &max_num_tilts 4
+
+module_name: Control Allocation
+
+parameters:
+    - group: Geometry
+      definitions:
+        CA_AIRFRAME:
+            description:
+                short: Airframe selection
+                long: |
+                  Defines which mixer implementation to use.
+                  Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators.
+
+                  'Custom' should only be used if noting else can be used.
+            type: enum
+            values:
+                0: Multirotor
+                1: Fixed-wing
+                2: Standard VTOL
+                3: Tiltrotor VTOL
+                4: Tailsitter VTOL
+                5: Rover (Ackermann)
+                6: Rover (Differential)
+                7: Motors (6DOF)
+                8: Multirotor with Tilt
+                9: Custom
+                10: Helicopter (tail ESC)
+                11: Helicopter (tail Servo)
+                12: Helicopter (Coaxial)
+                13: Rover (Mecanum)
+                14: Spacecraft 2D
+                15: Spacecraft 3D
+            default: 14
+
+        CA_METHOD:
+            description:
+                short: Control allocation method
+                long: |
+                  Selects the algorithm and desaturation method.
+                  If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
+            type: enum
+            values:
+                0: Pseudo-inverse with output clipping
+                1: Pseudo-inverse with metric allocation
+                2: Automatic
+            default: 1
+
+        CA_R_REV:
+            description:
+                short: Bidirectional/Reversible motors
+                long: |
+                  Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well.
+            type: bitmask
+            bit:
+                0: Motor 1
+                1: Motor 2
+                2: Motor 3
+                3: Motor 4
+                4: Motor 5
+                5: Motor 6
+                6: Motor 7
+                7: Motor 8
+                8: Motor 9
+                9: Motor 10
+                10: Motor 11
+                11: Motor 12
+            default: 0
+
+        # (SC) Thrusters
+        CA_THRUSTER_CNT:
+            description:
+                short: Total number of thrusters
+            type: enum
+            values:
+                0: '0'
+                1: '1'
+                2: '2'
+                3: '3'
+                4: '4'
+                5: '5'
+                6: '6'
+                7: '7'
+                8: '8'
+                9: '9'
+                10: '10'
+                11: '11'
+                12: '12'
+            default: 0
+        CA_THRUSTER${i}_PX:
+            description:
+                short: Position of thruster ${i} along X body axis
+            type: float
+            decimal: 2
+            increment: 0.1
+            unit: m
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: 0.0
+        CA_THRUSTER${i}_PY:
+            description:
+                short: Position of thruster ${i} along Y body axis
+            type: float
+            decimal: 2
+            increment: 0.1
+            unit: m
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: 0.0
+        CA_THRUSTER${i}_PZ:
+            description:
+                short: Position of thruster ${i} along Z body axis
+            type: float
+            decimal: 2
+            increment: 0.1
+            unit: m
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: 0.0
+        CA_THRUSTER${i}_AX:
+            description:
+                short: Axis of thruster ${i} thrust vector, X body axis component
+                long: Only the direction is considered (the vector is normalized).
+            type: float
+            decimal: 2
+            increment: 0.1
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: 0.0
+        CA_THRUSTER${i}_AY:
+            description:
+                short: Axis of thruster ${i} thrust vector, Y body axis component
+                long: Only the direction is considered (the vector is normalized).
+            type: float
+            decimal: 2
+            increment: 0.1
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: 0.0
+        CA_THRUSTER${i}_AZ:
+            description:
+                short: Axis of thruster ${i} thrust vector, Z body axis component
+                long: Only the direction is considered (the vector is normalized).
+            type: float
+            decimal: 2
+            increment: 0.1
+            num_instances: *max_num_thrusters
+            min: -100
+            max: 100
+            default: -1.0
+
+        CA_THRUSTER${i}_CT:
+            description:
+                short: Thrust coefficient of rotor ${i}
+                long: |
+                  The thrust coefficient if defined as Thrust = CT * u^2,
+                  where u (with value between actuator minimum and maximum)
+                  is the output signal sent to the motor controller.
+            type: float
+            decimal: 1
+            increment: 1
+            num_instances: *max_num_thrusters
+            min: 0
+            max: 100
+            default: 6.5
+
+# Mixer
+mixer:
+    actuator_types:
+        motor:
+            functions: 'Motor'
+            actuator_testing_values:
+                min: 0
+                max: 1
+                default_is_nan: true
+        DEFAULT:
+            actuator_testing_values:
+                min: -1
+                max: 1
+                default: -1
+
+    config:
+        param: CA_AIRFRAME
+        types:
+            14: # Spacecraft 2D
+                title: 'Spacecraft 2D'
+                actuators:
+                  - actuator_type: 'motor'
+                    count: 'CA_THRUSTER_CNT'  # count would be too long for 16 max size
+                    per_item_parameters:
+                        standard:
+                            position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ]
+                        extra:
+                          - name: 'CA_THRUSTER${i}_AX'
+                            label: 'Axis X'
+                            function: 'axisx'
+                            advanced: true
+                          - name: 'CA_THRUSTER${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                            advanced: true
+                          - name: 'CA_THRUSTER${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
+                            advanced: true
+
+            15: # Sapcecraft 3D
+                title: 'Spacecraft 3D'
+                actuators:
+                  - actuator_type: 'motor'
+                    count: 'CA_THRUSTER_CNT'
+                    per_item_parameters:
+                        standard:
+                            position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ]
+                        extra:
+                          - name: 'CA_THRUSTER${i}_AX'
+                            label: 'Axis X'
+                            function: 'axisx'
+                            advanced: true
+                          - name: 'CA_THRUSTER${i}_AY'
+                            label: 'Axis Y'
+                            function: 'axisy'
+                            advanced: true
+                          - name: 'CA_THRUSTER${i}_AZ'
+                            label: 'Axis Z'
+                            function: 'axisz'
+                            advanced: true
@@ -74,6 +74,9 @@ publications:
  - topic: /fmu/out/vehicle_trajectory_waypoint_desired
    type: px4_msgs::msg::VehicleTrajectoryWaypoint

+  - topic: /fmu/out/airspeed_validated
+    type: px4_msgs::msg::AirspeedValidated
+
 # Create uORB::Publication
 subscriptions:
  - topic: /fmu/in/register_ext_component_request
Author	SHA1	Message	Date
Balduin	96fc4a0673	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.	2025-02-18 13:03:45 +01:00
Balduin	122643af25	preflight check: fix range	2025-02-18 13:03:38 +01:00
Balduin	71afdb5680	preflight check: restore old nav_state only once previously it would always change _user_mode_intention to _prev_nav_state forever. now we change it only if we are in preflight check mode to not interfere with anything else later.	2025-02-18 11:08:12 +01:00
Balduin	0253f9798e	preflight check: send tilt control always even if we are not currently testing the tilt axis. then we send zero tilt rather than nothing.	2025-02-18 11:08:06 +01:00
Balduin	ad42a5b2f9	preflight check: more self explanatory code	2025-02-17 17:46:39 +01:00
Balduin	761810884e	preflight check: always send tiltrotor controls	2025-02-17 17:24:21 +01:00
Balduin	2a779663f6	preflight check: tilt collective actuation we "intercept" the tiltrotor_extra_controls message, which is read again right after in ActuatorEffectivenessTiltrotorVTOL::updateSetpoint.	2025-02-17 11:40:41 +01:00
Balduin	7816fd14b7	preflight check: restore prev nav state	2025-02-14 13:37:31 +01:00
Balduin	64d18f0a64	preflight check: first working version modifies torque setpoints directly via the matrix c.	2025-02-14 12:17:10 +01:00
Balduin	a3cca099a8	preflight check: set vehicle_control_mode flags not sure if all of these are needed, but with all of them the allocator runs, and with only flag_control_allocation_enabled it does not.	2025-02-14 10:59:42 +01:00
Balduin	01a65bfcf6	preflight check: base business logic - currently triggered via param COM_DO_CS_CHECK. ultimately this will be replaced by a mavlink cmd - new VehicleStatus.nav_state, NAVIGATION_STATE_CS_PREFLIGHT_CHECK. this is how commander tells ControlAllocator to conduct the check - (todo) within ControlAllocator we overwrite torque setpoints to do the check. additionally we can also control servos directly from there, in a different mode if that is needed.	2025-02-14 09:45:53 +01:00
Bertug Dilman	ce64263ce7	publish validated airspeed topic (#24302 ) * publish validated airspeed topic * fix typo	2025-02-07 14:44:48 +01:00
mahimayoga	69d95a6664	sf45: separate sensor yaw variable into FRD and sensor frames for clarity. Obstacle map is created in sensor frame, but scaling for vehicle orientation is done in vehicle FRD frame.	2025-02-07 13:28:17 +01:00
mahimayoga	093b379b6b	sf45/collision-prevention: replace repeated code with ObstacleMath library functions.	2025-02-07 13:28:17 +01:00
Pedro Roque	e7e76e2e21	Spacecraft build and bare control allocator (#24221 )	2025-02-06 23:54:24 -05:00
Marco Hauswirth	de1ade8eb8	sensors/vehicle_air_data: only trigger the sensor fallback when the baro_fault flag switches from 0 -> 1	2025-02-06 23:47:32 -05:00
Alexander Lerach	fd175d619c	boards/modalai/voxl2/target: remove trailing spaces, consistent tabs/spaces in files	2025-02-06 23:43:34 -05:00
bresch	8d296a50f9	FlightTask: properly initialize EKF reset counters This fixes a race condition when switching from a flight mode that is not a flight task (e.g.: stabilized). In this case, the reset counters were initialized to 0 and deltas were applied to the first setpoints if the EKF had any of its reset counters different from 0.	2025-02-05 13:21:32 -05:00
Junwoo Hwang	de650cab9e	boards/matek/h743-slim: change parameter path to MicroSD	2025-02-05 13:09:02 -05:00
Daniel Agar	bd2a009217	commander: remove MC nav test	2025-02-05 11:58:04 -05:00