control allocation: introduce parameter SYS_CTRL_ALLOC & unify build targets

ROMFS/px4fmu_common/init.d-posix/airframes/10017_iris_ctrlalloc

@@ -8,7 +8,8 @@
 #
 
 . ${R}etc/init.d/rc.mc_defaults
-. ${R}etc/init.d/rc.ctrlalloc
+
+param set-default SYS_CTRL_ALLOC 1
 
 param set-default MPC_USE_HTE 0
 

ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc

@@ -6,7 +6,8 @@
 #
 
 . ${R}etc/init.d/rc.mc_defaults
-. ${R}etc/init.d/rc.ctrlalloc
+
+param set-default SYS_CTRL_ALLOC 1
 
 param set-default MC_PITCHRATE_P 0.0800
 param set-default MC_PITCHRATE_I 0.0400

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -55,5 +55,4 @@ px4_add_romfs_files(
 	rc.vehicle_setup
 	rc.vtol_apps
 	rc.vtol_defaults
-	rc.ctrlalloc
 )

ROMFS/px4fmu_common/init.d/airframes/4018_s500_ctrlalloc

@@ -11,10 +11,11 @@
 #
 
 . ${R}etc/init.d/rc.mc_defaults
-. ${R}etc/init.d/rc.ctrlalloc
 
 set MIXER none
 
+param set-default SYS_CTRL_ALLOC 1
+
 param set-default MPC_USE_HTE 0
 
 param set-default VM_MASS 1.5

ROMFS/px4fmu_common/init.d/airframes/6003_hexa_x_ctrlalloc

@@ -11,8 +11,8 @@
 #
 
 . ${R}etc/init.d/rc.mc_defaults
-. ${R}etc/init.d/rc.ctrlalloc
 
+param set-default SYS_CTRL_ALLOC 1
 param set-default MPC_USE_HTE 0
 
 param set-default VM_MASS 1.5

ROMFS/px4fmu_common/init.d/rc.ctrlalloc

@@ -1,26 +0,0 @@
-#!/bin/sh
-#
-# Standard apps for new control allocation and controllers
-#
-# NOTE: Script variables are declared/initialized/unset in the rcS script.
-#
-
-#
-# Start angular velocity controller
-#
-# angular_velocity_controller start
-# mc_rate_control stop
-
-#
-# Start Control Allocator
-#
-control_allocator start
-
-#
-# Disable hover thrust estimator and prearming
-# These features are currently incompatible with control allocation
-#
-# TODO: fix
-#
-param set MPC_USE_HTE 0
-param set COM_PREARM_MODE 0

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -48,6 +48,23 @@ fi
 #                        End Estimator Group Selection                        #
 ###############################################################################
 
+if param compare SYS_CTRL_ALLOC 1
+then
+	#
+	# Start Control Allocator
+	#
+	control_allocator start
+
+	#
+	# Disable hover thrust estimator and prearming
+	# These features are currently incompatible with control allocation
+	#
+	# TODO: fix
+	#
+	param set MPC_USE_HTE 0
+	param set COM_PREARM_MODE 0
+fi
+
 #
 # Start Multicopter Rate Controller.
 #

boards/px4/fmu-v5/ctrlalloc.px4board

@@ -1 +0,0 @@
-CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/px4/fmu-v5/default.px4board

@@ -48,6 +48,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y

boards/px4/sitl/ctrlalloc.px4board

@@ -1 +0,0 @@
-CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/px4/sitl/default.px4board

@@ -11,6 +11,7 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y

src/lib/systemlib/system_params.c

@@ -265,3 +265,20 @@ PARAM_DEFINE_INT32(SYS_BL_UPDATE, 0);
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);
+
+
+/**
+ * Enable Dynamic Control Allocation
+ *
+ * If disabled, the existing mixing implementation is used.
+ * If enabled, dynamic control allocation with runtime configuration of the
+ * mixing and output functions is used.
+ *
+ * Note: this is work-in-progress and not all vehicle types are supported yet.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_CTRL_ALLOC, 0);
+

src/modules/logger/logged_topics.cpp

@@ -50,8 +50,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("actuator_controls_1", 100);
 	add_topic("actuator_controls_2", 100);
 	add_topic("actuator_controls_3", 100);
-	add_topic("actuator_controls_4", 100);
-	add_topic("actuator_controls_5", 100);
 	add_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_topic("airspeed_validated", 200);
@@ -115,13 +113,6 @@ void LoggedTopics::add_default_topics()
 	add_topic("vtol_vehicle_status", 200);
 	add_topic("wind", 1000);
 
-	// Control allocation topics
-	add_topic("vehicle_actuator_setpoint", 20);
-	add_topic("vehicle_angular_acceleration", 20);
-	add_topic("vehicle_angular_acceleration_setpoint", 20);
-	add_topic("vehicle_thrust_setpoint", 20);
-	add_topic("vehicle_torque_setpoint", 20);
-
 	// multi topics
 	add_topic_multi("actuator_outputs", 100, 3);
 	add_topic_multi("airspeed_wind", 1000);
@@ -188,6 +179,19 @@ void LoggedTopics::add_default_topics()
 	if (gps_dump_comm >= 1) {
 		add_topic("gps_dump");
 	}
+
+	int32_t sys_ctrl_alloc = 0;
+	param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
+
+	if (sys_ctrl_alloc >= 1) {
+		add_topic("actuator_motors", 100);
+		add_topic("actuator_servos", 100);
+		add_topic("vehicle_actuator_setpoint", 20);
+		add_topic("vehicle_angular_acceleration", 20);
+		add_topic("vehicle_angular_acceleration_setpoint", 20);
+		add_topic("vehicle_thrust_setpoint", 20);
+		add_topic("vehicle_torque_setpoint", 20);
+	}
 }
 
 void LoggedTopics::add_high_rate_topics()