feat: pushing modularity

.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/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 255
+
+# 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 501
+param set-default PWM_MAIN_FUNC2 502
+param set-default PWM_MAIN_FUNC3 503
+param set-default PWM_MAIN_FUNC4 504
+param set-default PWM_MAIN_FUNC5 505
+param set-default PWM_MAIN_FUNC6 506
+param set-default PWM_MAIN_FUNC7 507
+param set-default PWM_MAIN_FUNC8 508
+param set-default PWM_MAIN_FUNC9 509
+param set-default PWM_MAIN_FUNC10 510
+param set-default PWM_MAIN_FUNC11 511
+param set-default PWM_MAIN_FUNC12 512
+
+# 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 255
+
+# param set-default FW_ARSP_MODE 1
+
+# Auto to be provided by Custom Airframe
+param set-default CA_METHOD 3  # 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 501
+param set-default SIM_GZ_TH_FUNC2 502
+param set-default SIM_GZ_TH_FUNC3 503
+param set-default SIM_GZ_TH_FUNC4 504
+param set-default SIM_GZ_TH_FUNC5 505
+param set-default SIM_GZ_TH_FUNC6 506
+param set-default SIM_GZ_TH_FUNC7 507
+param set-default SIM_GZ_TH_FUNC8 508
+
+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

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

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/70000_spacecraft_2d

@@ -0,0 +1,151 @@
+#!/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 13
+param set-default MAV_TYPE 99
+
+param set-default CA_THRUSTER_CNT 8
+param set-default CA_R_REV 255
+
+# 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.sc_apps

@@ -0,0 +1,33 @@
+#!/bin/sh
+#
+# Standard apps for sr. Attitude/Position estimator, Attitude/Position control.
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+# 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

Tools/module_config/generate_actuators_metadata.py

@@ -364,6 +364,8 @@ def get_mixers(yaml_config, output_functions, verbose):
                     actuator['group-label'] = 'Motors'
                 elif actuator_conf['actuator_type'] == 'servo':
                     actuator['group-label'] = 'Servos'
+                elif actuator_conf['actuator_type'] == 'thruster':
+                    actuator['group-label'] = 'Thrusters'
                 else:
                     raise Exception('Missing group label for actuator type "{}"'.format(actuator_conf['actuator_type']))
 

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

msg/versioned/ActuatorMotors.msg

@@ -8,6 +8,7 @@ uint64 timestamp_sample	    # the timestamp the data this control response is ba
 uint16 reversible_flags     # bitset which motors are configured to be reversible
 
 uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
+uint16 ACTUATOR_FUNCTION_THRUSTER1 = 501
 
 uint8 NUM_CONTROLS = 12
 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,

src/lib/mixer_module/CMakeLists.txt

@@ -53,6 +53,7 @@ px4_add_library(mixer_module
 	functions/FunctionMotors.hpp
 	functions/FunctionParachute.hpp
 	functions/FunctionServos.hpp
+	functions/FunctionThrusters.hpp
 
 	actuator_test.cpp
 	actuator_test.hpp

src/lib/mixer_module/functions/FunctionThrusters.hpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Thruster1 ... ThrusterMax
+ */
+class FunctionThrusters : public FunctionProviderBase
+{
+public:
+	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::ThrusterMax - (int)OutputFunction::Thruster1 + 1,
+		      "Unexpected num thrusters");
+
+	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_THRUSTER1 == (int)OutputFunction::Thruster1,
+		      "Unexpected thruster idx");
+
+	FunctionThrusters(const Context &context) :
+		_topic(&context.work_item, ORB_ID(actuator_motors)),
+		_thrust_factor(context.thrust_factor)
+	{
+		for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
+			_data.control[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionThrusters(context); }
+
+	void update() override
+	{
+		if (_topic.update(&_data)) {
+			updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
+		}
+	}
+
+	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Thruster1]; }
+
+	bool allowPrearmControl() const override { return false; }
+
+	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
+
+	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
+
+	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
+	{
+
+	}
+
+	bool reversible(OutputFunction func) const override { return false; }
+
+private:
+	uORB::SubscriptionCallbackWorkItem _topic;
+	actuator_motors_s _data{};
+	const float &_thrust_factor;
+};

src/lib/mixer_module/mixer_module.hpp

@@ -44,6 +44,7 @@
 #include "functions/FunctionLandingGearWheel.hpp"
 #include "functions/FunctionManualRC.hpp"
 #include "functions/FunctionMotors.hpp"
+#include "functions/FunctionThrusters.hpp"
 #include "functions/FunctionParachute.hpp"
 #include "functions/FunctionServos.hpp"
 

src/lib/mixer_module/output_functions.yaml

@@ -10,6 +10,9 @@ functions:
     Motor:
       start: 101
       count: 12
+    Thruster:
+      start: 501
+      count: 12
     Servo:
       start: 201
       count: 8

src/modules/control_allocator/ControlAllocator.cpp

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

src/modules/control_allocator/ControlAllocator.hpp

@@ -158,6 +158,8 @@ private:
 		HELICOPTER_TAIL_ESC = 10,
 		HELICOPTER_TAIL_SERVO = 11,
 		HELICOPTER_COAXIAL = 12,
+		SPACECRAFT_2D = 13,
+		SPACECRAFT_3D = 14,
 	};
 
 	enum class FailureMode {

src/modules/control_allocator/module.yaml

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

src/modules/spacecraft/CMakeLists.txt

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

src/modules/spacecraft/Kconfig

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

src/modules/spacecraft/SpacecraftHandler.cpp

@@ -0,0 +1,343 @@
+/****************************************************************************
+ *
+ *   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"
+
+
+SpacecraftHandler::SpacecraftHandler() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+	_rover_differential_setpoint_pub.advertise();
+}
+
+bool SpacecraftHandler::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void SpacecraftHandler::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+}
+
+void SpacecraftHandler::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	updateSubscriptions();
+
+	// Generate and publish attitude, rate and speed setpoints
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	switch (_nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				_thrust_setpoint(0) = math::constrain((manual_control_setpoint.pitch * _manual_force_max), -1.f, 1.f);
+				_thrust_setpoint(1) = math::constrain((manual_control_setpoint.roll * _manual_force_max), -1.f, 1.f);
+				_thrust_setpoint(2) = 0.0;
+
+				_torque_setpoint(0) = _torque_setpoint(1) = 0.0;
+				_torque_setpoint(2) = math::constrain((manual_control_setpoint.yaw * _manual_torque_max), -1.f, 1.f);
+				// publish thrust and torque setpoints
+				vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
+				vehicle_torque_setpoint_s vehicle_torque_setpoint{};
+
+				_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+				_torque_setpoint.copyTo(vehicle_torque_setpoint.xyz);
+
+				vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+				vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
+
+				vehicle_torque_setpoint.timestamp = hrt_absolute_time();
+				vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
+
+				_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
+				_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
+
+				updateActuatorControlsStatus(vehicle_torque_setpoint, dt);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = NAN;
+				rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
+					if (!_yaw_ctl) {
+						_stab_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_differential_setpoint_s rover_differential_setpoint{};
+				rover_differential_setpoint.timestamp = timestamp;
+				rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
+				rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
+						STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_differential_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+
+				} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
+					if (!_yaw_ctl) {
+						_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
+						_pos_ctl_start_position_ned = _curr_pos_ned;
+						_yaw_ctl = true;
+					}
+
+					// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+					const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
+										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
+					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
+							rover_differential_setpoint.forward_speed_setpoint) *
+									     vector_scaling * _pos_ctl_course_direction;
+					// Calculate yaw setpoint
+					const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
+								   _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
+					rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
+							yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
+					rover_differential_setpoint.yaw_rate_setpoint = NAN;
+
+				}
+
+				_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+			}
+
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
+		rover_differential_setpoint_s rover_differential_setpoint{};
+		rover_differential_setpoint.forward_speed_setpoint = NAN;
+		rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
+		rover_differential_setpoint.yaw_rate_setpoint = NAN;
+		rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f;
+		rover_differential_setpoint.yaw_setpoint = NAN;
+		_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
+		break;
+	}
+
+	if (!_armed) { // Reset when disarmed
+		_rover_differential_control.resetControllers();
+		_yaw_ctl = false;
+	}
+
+	_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
+
+}
+
+void SpacecraftHandler::updateSubscriptions()
+{
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+		updateParams();
+	}
+
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
+			_rover_differential_control.resetControllers();
+			_yaw_ctl = false;
+		}
+
+		_nav_state = vehicle_status.nav_state;
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+	}
+
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
+		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
+	}
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
+	}
+
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+	}
+}
+
+int SpacecraftHandler::task_spawn(int argc, char *argv[])
+{
+	SpacecraftHandler *instance = new SpacecraftHandler();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+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("sc_control_allocator", "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);
+}

src/modules/spacecraft/SpacecraftHandler.hpp

@@ -0,0 +1,135 @@
+/****************************************************************************
+ *
+ *   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.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>
+
+// Constants
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
+
+class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
+{
+
+public:
+	SpacecraftHandler();
+	~SpacecraftHandler() override = default;
+
+	/** @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);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+private:
+	void Run() override;
+
+	/**
+	 * @brief Update uORB subscriptions.
+	 */
+	void updateSubscriptions();
+
+	void updateActuatorControlsStatus(const vehicle_torque_setpoint_s &vehicle_torque_setpoint, float dt);
+
+	// uORB Subscriptions
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	// uORB Publications
+	uORB::Publication<rover_differential_setpoint_s> _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)};
+
+	// Instances
+	SpacecraftHandlerGuidance _rover_differential_guidance{this};
+	SpacecraftHandlerControl _rover_differential_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
+
+	// Variables
+	Vector2f _curr_pos_ned{};
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+	float _vehicle_yaw{0.f};
+	float _max_yaw_rate{0.f};
+	int _nav_state{0};
+	bool _armed{false};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
+	float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
+	Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
+	Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
+
+
+};

src/modules/spacecraft/module.yaml

@@ -0,0 +1,187 @@
+__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:
+
+        SC_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
+
+        # (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:
+        thruster:
+            functions: 'Thruster'
+            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: 'thruster'
+                    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: 'thruster'
+                    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