uavcan: add dynamic mixing support

src/drivers/uavcan/CMakeLists.txt

@@ -160,6 +160,9 @@ px4_add_module(
 		sensors/cbat.cpp
 		sensors/ice_status.cpp
 
+	MODULE_CONFIG
+		module.yaml
+
 	DEPENDS
 		px4_uavcan_dsdlc
 

src/drivers/uavcan/actuators/esc.cpp

@@ -79,14 +79,6 @@ UavcanEscController::init()
 void
 UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
 {
-	if (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) {
-		num_outputs = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
-	}
-
-	if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) {
-		num_outputs = esc_status_s::CONNECTED_ESC_MAX;
-	}
-
 	/*
 	 * Rate limiting - we don't want to congest the bus
 	 */

src/drivers/uavcan/actuators/esc.hpp

@@ -57,10 +57,13 @@
 class UavcanEscController
 {
 public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+	static constexpr int MAX_ACTUATORS = esc_status_s::CONNECTED_ESC_MAX;
 	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
 	static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX;
 
+	static_assert(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize >= MAX_ACTUATORS, "Too many actuators");
+
+
 	UavcanEscController(uavcan::INode &node);
 	~UavcanEscController();
 

src/drivers/uavcan/module.yaml

@@ -0,0 +1,12 @@
+
+module_name: UAVCAN
+actuator_output:
+  output_groups:
+    - param_prefix: UAVCAN_EC
+      channel_label: 'UAVCAN ESC'
+      standard_params:
+        min: { min: 0, max: 8191, default: 1 }
+        max: { min: 0, max: 8191, default: 8191 }
+        failsafe: { min: 0, max: 8191 }
+      num_channels: 8
+

src/drivers/uavcan/uavcan_main.cpp

@@ -561,6 +561,7 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	}
 
 	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
+	_instance->_mixing_interface.ScheduleNow();
 
 	return OK;
 }
@@ -661,9 +662,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		PX4_DEBUG("sensor bridge '%s' init ok", br->get_name());
 	}
 
-	_mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
-	_mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
-
 	// Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions
 	if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE
 	    || UavcanEscController::max_output_value() > (int)UINT16_MAX) {
@@ -671,11 +669,19 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return -EINVAL;
 	}
 
-	_mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
+	_mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
+
+	if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
+		// these are configurable with dynamic mixing
+		_mixing_interface.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
+		_mixing_interface.mixingOutput().setAllMaxValues(UavcanEscController::max_output_value());
+
+		param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
+		enable_idle_throttle_when_armed(true);
+	}
+
 	_mixing_interface.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
 
-	param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed_param);
-	enable_idle_throttle_when_armed(true);
 
 	/*  Start the Node   */
 	return _node.start();
@@ -831,9 +837,11 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
 {
 	value &= _idle_throttle_when_armed_param > 0;
 
-	if (value != _idle_throttle_when_armed) {
-		_mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0);
-		_idle_throttle_when_armed = value;
+	if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
+		if (value != _idle_throttle_when_armed) {
+			_mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0);
+			_idle_throttle_when_armed = value;
+		}
 	}
 }
 
@@ -931,8 +939,15 @@ void UavcanMixingInterface::mixerChanged()
 {
 	int rotor_count = 0;
 
-	if (_mixing_output.mixers()) {
-		rotor_count = _mixing_output.mixers()->get_multirotor_count();
+	if (_mixing_output.useDynamicMixing()) {
+		for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
+			rotor_count += _mixing_output.isFunctionSet(i);
+		}
+
+	} else {
+		if (_mixing_output.mixers()) {
+			rotor_count = _mixing_output.mixers()->get_multirotor_count();
+		}
 	}
 
 	_esc_controller.set_rotor_count(rotor_count);

src/drivers/uavcan/uavcan_main.hpp

@@ -104,7 +104,7 @@ private:
 	friend class UavcanNode;
 	pthread_mutex_t &_node_mutex;
 	UavcanEscController &_esc_controller;
-	MixingOutput _mixing_output{"UAVCAN_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	MixingOutput _mixing_output{"UAVCAN_EC", UavcanEscController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 };
 
 /**