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_esc.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().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_esc.mixingOutput().unregister();
-		_mixing_interface.ScheduleClear();
+		_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,14 +926,14 @@ 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();
@@ -935,7 +941,7 @@ void UavcanMixingInterface::Run()
 	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
+	printf("ESC outputs:\n");
-	_mixing_interface.mixingOutput().printStatus();
+	_mixing_interface_esc.mixingOutput().printStatus();
+	printf("Servo outputs:\n");
+	_mixing_interface_servo.mixingOutput().printStatus();
 
 	printf("\n");
 

src/drivers/uavcan/uavcan_main.hpp

@@ -54,6 +54,7 @@
 #include "allocator.hpp"
 #include "actuators/esc.hpp"
 #include "actuators/hardpoint.hpp"
+#include "actuators/servo.hpp"
 #include "sensors/sensor_bridge.hpp"
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
@@ -77,17 +78,17 @@ using namespace time_literals;
 class UavcanNode;
 
 /**
- * UAVCAN mixing class.
+ * UAVCAN mixing class for ESCs.
- * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling
+ * It is separate from UavcanNode to have separate WorkItems and therefore allowing independent scheduling
- * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at
+ * (I.e. UavcanMixingInterfaceESC runs upon actuator_control updates, whereas UavcanNode runs at
  * a fixed rate or upon bus updates).
- * Both work items are expected to run on the same work queue.
+ * All work items are expected to run on the same work queue.
  */
-class UavcanMixingInterface : public OutputModuleInterface
+class UavcanMixingInterfaceESC : public OutputModuleInterface
 {
 public:
-	UavcanMixingInterface(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
+	UavcanMixingInterfaceESC(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller)
-		: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
+		: OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::uavcan),
 		  _node_mutex(node_mutex),
 		  _esc_controller(esc_controller) {}
 
@@ -107,6 +108,35 @@ private:
 	MixingOutput _mixing_output{"UAVCAN_EC", UavcanEscController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 };
 
+/**
+ * UAVCAN mixing class for Servos.
+ * It is separate from UavcanNode to have separate WorkItems and therefore allowing independent scheduling
+ * (I.e. UavcanMixingInterfaceServo runs upon actuator_control updates, whereas UavcanNode runs at
+ * a fixed rate or upon bus updates).
+ * All work items are expected to run on the same work queue.
+ */
+class UavcanMixingInterfaceServo : public OutputModuleInterface
+{
+public:
+	UavcanMixingInterfaceServo(pthread_mutex_t &node_mutex, UavcanServoController &servo_controller)
+		: OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::uavcan),
+		  _node_mutex(node_mutex),
+		  _servo_controller(servo_controller) {}
+
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
+			   unsigned num_outputs, unsigned num_control_groups_updated) override;
+
+	MixingOutput &mixingOutput() { return _mixing_output; }
+
+protected:
+	void Run() override;
+private:
+	friend class UavcanNode;
+	pthread_mutex_t &_node_mutex;
+	UavcanServoController &_servo_controller;
+	MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+};
+
 /**
  * A UAVCAN node.
  */
@@ -204,7 +234,9 @@ private:
 	pthread_mutex_t			_node_mutex;
 	px4_sem_t			_server_command_sem;
 	UavcanEscController		_esc_controller;
-	UavcanMixingInterface 		_mixing_interface{_node_mutex, _esc_controller};
+	UavcanServoController		_servo_controller;
+	UavcanMixingInterfaceESC 	_mixing_interface_esc{_node_mutex, _esc_controller};
+	UavcanMixingInterfaceServo 	_mixing_interface_servo{_node_mutex, _servo_controller};
 	UavcanHardpointController	_hardpoint_controller;
 	UavcanBeep			_beep_controller;
 	UavcanSafetyState         	_safety_state_controller;