diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 8915fe8932..f60f5942d9 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/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 diff --git a/src/drivers/uavcan/actuators/servo.cpp b/src/drivers/uavcan/actuators/servo.cpp new file mode 100644 index 0000000000..56fdea4bba --- /dev/null +++ b/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 +#include + +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); +} diff --git a/src/drivers/uavcan/actuators/servo.hpp b/src/drivers/uavcan/actuators/servo.hpp new file mode 100644 index 0000000000..f0f1e97fb6 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +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_pub_array_cmd; +}; diff --git a/src/drivers/uavcan/module.yaml b/src/drivers/uavcan/module.yaml index fd12f0db56..c738c38387 100644 --- a/src/drivers/uavcan/module.yaml +++ b/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 diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 14be0f46b7..3c5868da8a 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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,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 - _mixing_interface.mixingOutput().printStatus(); + printf("ESC outputs:\n"); + _mixing_interface_esc.mixingOutput().printStatus(); + printf("Servo outputs:\n"); + _mixing_interface_servo.mixingOutput().printStatus(); printf("\n"); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 3da8024afc..7b40213667 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/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 @@ -77,17 +78,17 @@ using namespace time_literals; class UavcanNode; /** - * UAVCAN mixing class. - * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling - * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at + * UAVCAN mixing class for ESCs. + * It is separate from UavcanNode to have separate WorkItems and therefore allowing independent scheduling + * (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) - : OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan), + UavcanMixingInterfaceESC(pthread_mutex_t &node_mutex, UavcanEscController &esc_controller) + : 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;