From 04ea1cf5c60e52244508f08668863dcdb2af295e Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Mon, 22 Feb 2021 08:17:34 -0800 Subject: [PATCH] uavcan_v1: Add to EscClient implementation Add outline of EscServer implementation Add some comments and cleanup --- src/drivers/uavcan_v1/Actuators/EscClient.hpp | 99 +++++++++++++++---- src/drivers/uavcan_v1/Actuators/EscServer.hpp | 74 ++++++++++++++ src/drivers/uavcan_v1/Uavcan.cpp | 1 + src/drivers/uavcan_v1/Uavcan.hpp | 9 +- 4 files changed, 159 insertions(+), 24 deletions(-) create mode 100644 src/drivers/uavcan_v1/Actuators/EscServer.hpp diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index 5270957cce..3be32fac9a 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -32,46 +32,101 @@ ****************************************************************************/ /** - * @file esc.hpp + * @file EscClient.hpp * - * UAVCAN <--> ORB bridge for ESC messages: - * uavcan.equipment.esc.RawCommand - * uavcan.equipment.esc.RPMCommand - * uavcan.equipment.esc.Status + * Client-side implementation of DS-15 specification ESC service + * + * Publishes the following UAVCAN v1 messages: + * reg.drone.service.actuator.common.sp.Value8.0.1 + * reg.drone.service.common.Readiness.0.1 + * + * Subscribes to the following UAVCAN v1 messages: + * reg.drone.service.actuator.common.Feedback.0.1 + * reg.drone.service.actuator.common.Status.0.1 * * @author Pavel Kirienko + * @author Jacob Crabill */ #pragma once #include #include -#include +#include #include #include #include +// DS-15 Specification Messages #include #include -class UavcanEscController +/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status +class UavcanEscController : public UavcanPublication { public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable - static constexpr uint16_t DISARMED_OUTPUT_VALUE = UINT16_MAX; - /// TODO: derive from Publisher class to handle port-ID setting via parameter - UavcanEscController(CanardInstance &ins, const CanardPortID &baseID) : - _canard_instance(ins), _base_port_id(baseID) {}; + UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanPublication(ins, pmgr, uavcan_pname) { }; ~UavcanEscController() {}; + void update() override + { + if (_armed_sub.updated()) { + actuator_armed_s new_arming; + _armed_sub.update(&new_arming); + + if (new_arming.armed != _armed.armed) { + _armed = new_arming; + + // Only publish if we have a valid publication ID set + if (_port_id == 0) { + return; + } + + reg_drone_service_common_Readiness_0_1 msg_arming {}; + + if (_armed.armed) { + msg_arming.value = reg_drone_service_common_Readiness_0_1_ENGAGED; + + } else if (_armed.prearmed) { + msg_arming.value = reg_drone_service_common_Readiness_0_1_STANDBY; + + } else { + msg_arming.value = reg_drone_service_common_Readiness_0_1_SLEEP; + } + + uint8_t arming_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = arming_pid, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _arming_transfer_id, + .payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &arming_payload_buffer, + }; + + int result = reg_drone_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + } + } + }; + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { - if (_base_port_id > 0) { - /// TODO: Update readiness - // reg_drone_service_common_Readiness_0_1 msg_arming; + if (_port_id > 0) { reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; for (uint8_t i = 0; i < num_outputs; i++) { @@ -84,9 +139,9 @@ public: .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, - .port_id = _base_port_id, // This is the subject-ID. + .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _esc_setpoint_transfer_id, + .transfer_id = _transfer_id, .payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, .payload = &esc_sp_payload_buffer, }; @@ -96,7 +151,7 @@ public: if (result == 0) { // set the data ready in the buffer and chop if needed - ++_esc_setpoint_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. result = canardTxPush(&_canard_instance, &transfer); } } @@ -113,8 +168,10 @@ private: */ void esc_status_sub_cb(const CanardTransfer &msg); - CanardInstance &_canard_instance; - CanardPortID _base_port_id {0}; uint8_t _rotor_count {0}; - CanardTransferID _esc_setpoint_transfer_id {0}; + + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + actuator_armed_s _armed {}; + + CanardTransferID _arming_transfer_id; }; diff --git a/src/drivers/uavcan_v1/Actuators/EscServer.hpp b/src/drivers/uavcan_v1/Actuators/EscServer.hpp new file mode 100644 index 0000000000..af2ba1338c --- /dev/null +++ b/src/drivers/uavcan_v1/Actuators/EscServer.hpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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. + * + ****************************************************************************/ + +/** + * @file EscServer.hpp + * + * Server-side implementation of DS-15 specification ESC service + * (Used for CANNode control of an ESC via PWM output) + * + * Subscribes to the following UAVCAN v1 messages: + * reg.drone.service.actuator.common.sp.Value8.0.1 + * reg.drone.service.common.Readiness.0.1 + * + * Publishes to the following UAVCAN v1 messages: + * reg.drone.service.actuator.common.Feedback.0.1 + * reg.drone.service.actuator.common.Status.0.1 + * + * And publishes to the following uORB topics: + * actuator_armed + * output_control_mc + * + * @author Jacob Crabill + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include + +// DS-15 Specification Messages +#include +#include + +#include "../Publishers/Publisher.hpp" + +class EscServer : public UavcanSubscription +{ +/// TODO +} diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 4f26508085..23ac0b311d 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -403,6 +403,7 @@ void UavcanNode::print_info() } _mixing_output.printInfo(); + _esc_controller.printInfo(); pthread_mutex_unlock(&_node_mutex); } diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 5575c18898..85e83f8ebc 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -237,7 +237,11 @@ private: UavcanGnssPublication _gps_pub {_canard_instance, _param_manager, "uavcan.pub.gps.0.id"}; - UavcanPublication *_publishers[1] {&_gps_pub}; + UavcanEscController _esc_controller {_canard_instance, _param_manager, "uavcan.pub.esc.0.id"}; + + // Publication objects: Any object used to bridge a uORB message to a UAVCAN message + /// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers + UavcanPublication *_publishers[2] {&_gps_pub, &_esc_controller}; UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"}; UavcanGnssSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"}; @@ -245,9 +249,8 @@ private: UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"}; UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"}; + // Subscription objects: Any object used to bridge a UAVCAN message to a uORB message UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List - UavcanEscController _esc_controller {_canard_instance, 22}; //// TODO - UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; };