uavcan: implement servo outputs

src/drivers/uavcan/CMakeLists.txt

@@ -144,6 +144,7 @@ px4_add_module(
 		# Actuators
 		actuators/esc.cpp
 		actuators/hardpoint.cpp
+		actuators/servo.cpp
 
 		# Sensors
 		sensors/sensor_bridge.cpp

src/drivers/uavcan/actuators/servo.cpp

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 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 "servo.hpp"
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+UavcanServoController::UavcanServoController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_array_cmd(node)
+{
+	_uavcan_pub_array_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
+}
+
+void
+UavcanServoController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+{
+	uavcan::equipment::actuator::ArrayCommand msg;
+
+	for (unsigned i = 0; i < num_outputs; ++i) {
+		uavcan::equipment::actuator::Command cmd;
+		cmd.actuator_id = i;
+		cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS;
+		cmd.command_value = (float)outputs[i] / 500.f - 1.f; // [-1, 1]
+
+		msg.commands.push_back(cmd);
+	}
+
+	_uavcan_pub_array_cmd.broadcast(msg);
+}

src/drivers/uavcan/actuators/servo.hpp

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 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 <uavcan/uavcan.hpp>
+#include <uavcan/equipment/actuator/Status.hpp>
+#include <uavcan/equipment/actuator/ArrayCommand.hpp>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/actuator_outputs.h>
+#include <drivers/drv_hrt.h>
+#include <lib/mixer_module/mixer_module.hpp>
+
+class UavcanServoController
+{
+public:
+	static constexpr int MAX_ACTUATORS = 8;
+	static constexpr unsigned MAX_RATE_HZ = 50;
+	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 6;	///< 0..31, inclusive, 0 - highest, 31 - lowest
+
+	UavcanServoController(uavcan::INode &node);
+	~UavcanServoController() = default;
+
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs);
+
+private:
+	uavcan::INode								&_node;
+	uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand> _uavcan_pub_array_cmd;
+};

src/drivers/uavcan/module.yaml

@@ -9,4 +9,12 @@ actuator_output:
         max: { min: 0, max: 8191, default: 8191 }
         failsafe: { min: 0, max: 8191 }
       num_channels: 8
+    - param_prefix: UAVCAN_SV
+      channel_label: 'UAVCAN Servo'
+      standard_params:
+        disarmed: { min: 0, max: 1000, default: 500 }
+        min: { min: 0, max: 1000, default: 0 }
+        max: { min: 0, max: 1000, default: 1000 }
+        failsafe: { min: 0, max: 1000 }
+      num_channels: 8
 

src/drivers/uavcan/uavcan_main.cpp

@@ -82,6 +82,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	ModuleParams(nullptr),
 	_node(can_driver, system_clock, _pool_allocator),
 	_esc_controller(_node),
+	_servo_controller(_node),
 	_hardpoint_controller(_node),
 	_beep_controller(_node),
 	_safety_state_controller(_node),
@@ -107,6 +108,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 
 	/* _server_command_sem use case is a signal */
 	px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
+
+	_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
+	_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
+
 }
 
 UavcanNode::~UavcanNode()
@@ -413,7 +418,8 @@ UavcanNode::get_param(int remote_node_id, const char *name)
 void
 UavcanNode::update_params()
 {
-	_mixing_interface.updateParams();
+	_mixing_interface_esc.updateParams();
+	_mixing_interface_servo.updateParams();
 }
 
 int
@@ -561,7 +567,8 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	}
 
 	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
-	_instance->_mixing_interface.ScheduleNow();
+	_instance->_mixing_interface_esc.ScheduleNow();
+	_instance->_mixing_interface_servo.ScheduleNow();
 
 	return OK;
 }
@@ -669,20 +676,17 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return -EINVAL;
 	}
 
-	_mixing_interface.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
+	_mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE);
 
-	if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
+	if (!_mixing_interface_esc.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());
+		_mixing_interface_esc.mixingOutput().setAllMinValues(0); // Can be changed to 1 later, according to UAVCAN_ESC_IDLT
+		_mixing_interface_esc.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);
-
-
 	/*  Start the Node   */
 	return _node.start();
 }
@@ -788,7 +792,7 @@ UavcanNode::Run()
 	node_spin_once(); // expected to be non-blocking
 
 	// Check arming state
-	const actuator_armed_s &armed = _mixing_interface.mixingOutput().armed();
+	const actuator_armed_s &armed = _mixing_interface_esc.mixingOutput().armed();
 	enable_idle_throttle_when_armed(!armed.soft_stop);
 
 	// check for parameter updates
@@ -824,8 +828,10 @@ UavcanNode::Run()
 	pthread_mutex_unlock(&_node_mutex);
 
 	if (_task_should_exit.load()) {
-		_mixing_interface.mixingOutput().unregister();
-		_mixing_interface.ScheduleClear();
+		_mixing_interface_esc.mixingOutput().unregister();
+		_mixing_interface_esc.ScheduleClear();
+		_mixing_interface_servo.mixingOutput().unregister();
+		_mixing_interface_servo.ScheduleClear();
 		ScheduleClear();
 		teardown();
 		_instance = nullptr;
@@ -837,9 +843,9 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
 {
 	value &= _idle_throttle_when_armed_param > 0;
 
-	if (!_mixing_interface.mixingOutput().useDynamicMixing()) {
+	if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
 		if (value != _idle_throttle_when_armed) {
-			_mixing_interface.mixingOutput().setAllMinValues(value ? 1 : 0);
+			_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
 			_idle_throttle_when_armed = value;
 		}
 	}
@@ -867,14 +873,14 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_interface.mixingOutput().resetMixerThreadSafe();
+		_mixing_interface_esc.mixingOutput().resetMixerThreadSafe();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_interface.mixingOutput().loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_interface_esc.mixingOutput().loadMixerThreadSafe(buf, buflen);
 		}
 		break;
 
@@ -920,22 +926,22 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 }
 
 
-bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
 	_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
 	return true;
 }
 
-void UavcanMixingInterface::Run()
+void UavcanMixingInterfaceESC::Run()
 {
 	pthread_mutex_lock(&_node_mutex);
 	_mixing_output.update();
 	_mixing_output.updateSubscriptions(false);
 	pthread_mutex_unlock(&_node_mutex);
 }
 
-void UavcanMixingInterface::mixerChanged()
+void UavcanMixingInterfaceESC::mixerChanged()
 {
 	int rotor_count = 0;
 
@@ -953,6 +959,21 @@ void UavcanMixingInterface::mixerChanged()
 	_esc_controller.set_rotor_count(rotor_count);
 }
 
+bool UavcanMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+		unsigned num_control_groups_updated)
+{
+	_servo_controller.update_outputs(stop_motors, outputs, num_outputs);
+	return true;
+}
+
+void UavcanMixingInterfaceServo::Run()
+{
+	pthread_mutex_lock(&_node_mutex);
+	_mixing_output.update();
+	_mixing_output.updateSubscriptions(false);
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void
 UavcanNode::print_info()
 {
@@ -994,8 +1015,10 @@ UavcanNode::print_info()
 
 	printf("\n");
 
-	// ESC mixer status
-	_mixing_interface.mixingOutput().printStatus();
+	printf("ESC outputs:\n");
+	_mixing_interface_esc.mixingOutput().printStatus();
+	printf("Servo outputs:\n");
+	_mixing_interface_servo.mixingOutput().printStatus();
 
 	printf("\n");