diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index b8c672eeb9..3bc56be4eb 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -107,10 +107,11 @@ public: private: - const UavcanParamBinder _uavcan_params[11] { + const UavcanParamBinder _uavcan_params[12] { {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.actuator_outputs.0.id", "UCAN1_ACTR_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.esc.0.id", "UCAN1_ESC0_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 1a444d7485..1a24fa0c67 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -80,7 +80,7 @@ public: // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; _param_manager.GetParamByName(uavcan_param, value); - int32_t new_id = value.integer32.value.elements[0]; + uint16_t new_id = value.natural16.value.elements[0]; // Allow, for example, a default PX4 param value of '-1' to disable publication if (!isValidPortId(new_id)) { diff --git a/src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp b/src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp new file mode 100644 index 0000000000..42b80ac72c --- /dev/null +++ b/src/drivers/uavcan_v1/Publishers/uORB/actuator_outputs.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file actuator_outputs.hpp + * +* Defines uORB over UAVCANv1 actuator_outputs publisher + * + * @author Peter van der Perk + */ + +#pragma once + +#include + +#include "../Publisher.hpp" + +class UORB_over_UAVCAN_actuator_outputs_Publisher : public UavcanPublisher +{ +public: + UORB_over_UAVCAN_actuator_outputs_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, "actuator_outputs", instance) + {}; + + // Update the uORB Subscription and broadcast a UAVCAN message + // FIXME think about update and limiting + virtual void update() override + { + // Not sure if actuator_armed is a good indication of readiness but seems close to it + if (_actuator_outputs_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) { + actuator_outputs_s actuator_msg {}; + _actuator_outputs_sub.update(&actuator_msg); + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + .payload_size = actuator_payload_size(&actuator_msg), + .payload = &actuator_msg, + }; + + // set the data ready in the buffer and chop if needed + ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + canardTxPush(&_canard_instance, &transfer); + } + }; + +private: + + // Remove unvalid output & padding from payload_size to save bandwidth + size_t actuator_payload_size(actuator_outputs_s *msg) + { + return sizeof(struct actuator_outputs_s) - sizeof(msg->_padding0) - + ((sizeof(msg->output) / sizeof(msg->output[0]) - msg->noutputs) * sizeof(msg->output[0])); + } + + uORB::Subscription _actuator_outputs_sub{ORB_ID(actuator_outputs)}; +}; diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 59889cf30d..217bac839f 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -59,6 +59,7 @@ #include "Publishers/Publisher.hpp" #include "Publishers/Gnss.hpp" +#include "Publishers/uORB/actuator_outputs.hpp" #include "NodeManager.hpp" @@ -183,11 +184,13 @@ private: UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager}; + UORB_over_UAVCAN_actuator_outputs_Publisher _actuator_pub {_canard_instance, _param_manager}; // uORB + UavcanEscController _esc_controller {_canard_instance, _param_manager}; // 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 - UavcanPublisher *_publishers[2] {&_gps_pub, &_esc_controller}; + UavcanPublisher *_publishers[3] {&_gps_pub, &_esc_controller, &_actuator_pub}; UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index f1241fc761..0d37cdc7cc 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -169,3 +169,12 @@ PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1); * @group UAVCAN v1 */ PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1); + +/** + * actuator_outputs uORB over UAVCAN v1 port ID. + * + * @min -1 + * @max 6143 + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1);