VehicleControlMode: add flag_control_allocation_enabled

Allows (external) modes to directly publish actuator_{motors,servos}.

msg/VehicleControlMode.msg

@@ -14,3 +14,4 @@ bool flag_control_position_enabled		# true if position is controlled
 bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
+bool flag_control_allocation_enabled		# true if control allocation is enabled

src/modules/commander/ModeUtil/control_mode.cpp

@@ -47,6 +47,7 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
 			   vehicle_control_mode_s &vehicle_control_mode)
 {
 	vehicle_control_mode.flag_armed = armed;
+	vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default
 
 	switch (nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:

src/modules/control_allocator/ControlAllocator.cpp

@@ -359,6 +359,14 @@ ControlAllocator::Run()
 		}
 	}
 
+	{
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
+			_publish_controls = vehicle_control_mode.flag_control_allocation_enabled;
+		}
+	}
+
 	// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
 	const hrt_abstime now = hrt_absolute_time();
 	const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@@ -641,6 +649,10 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 void
 ControlAllocator::publish_actuator_controls()
 {
+	if (!_publish_controls) {
+		return;
+	}
+
 	actuator_motors_s actuator_motors;
 	actuator_motors.timestamp = hrt_absolute_time();
 	actuator_motors.timestamp_sample = _timestamp_sample;

src/modules/control_allocator/ControlAllocator.hpp

@@ -73,6 +73,7 @@
 #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>
@@ -186,10 +187,12 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
 
 	matrix::Vector3f _torque_sp;
 	matrix::Vector3f _thrust_sp;
+	bool _publish_controls{true};
 
 	// Reflects motor failures that are currently handled, not motor failures that are reported.
 	// For example, the system might report two motor failures, but only the first one is handled by CA