From 35f822fca6f27c61e14f20a48ff1ad8775f35f95 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Sun, 21 Feb 2021 13:38:14 -0800 Subject: [PATCH] uavcan_v1: Working ESC setpoint pub/sub --- ROMFS/px4fmu_common/init.d/rcS | 5 + src/drivers/uavcan_v1/Actuators/EscClient.hpp | 119 ++++++++++++++++++ src/drivers/uavcan_v1/ParamManager.hpp | 3 +- src/drivers/uavcan_v1/Subscribers/Esc.hpp | 92 ++++++++++++++ src/drivers/uavcan_v1/Uavcan.cpp | 2 + src/drivers/uavcan_v1/Uavcan.hpp | 6 +- src/drivers/uavcan_v1/parameters.c | 1 + 7 files changed, 225 insertions(+), 3 deletions(-) create mode 100644 src/drivers/uavcan_v1/Actuators/EscClient.hpp create mode 100644 src/drivers/uavcan_v1/Subscribers/Esc.hpp diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3c49fee229..03da8d7b5e 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -427,6 +427,11 @@ else fi fi + if param greater -s UAVCAN_V1_ENABLE 0 + then + uavcan_v1 start + fi + # # Optional board mavlink streams: rc.board_mavlink # diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp new file mode 100644 index 0000000000..64d4b315ae --- /dev/null +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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 esc.hpp + * + * UAVCAN <--> ORB bridge for ESC messages: + * uavcan.equipment.esc.RawCommand + * uavcan.equipment.esc.RPMCommand + * uavcan.equipment.esc.Status + * + * @author Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +class UavcanEscController +{ +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; + + UavcanEscController(CanardInstance &ins, const CanardPortID &baseID) : + _canard_instance(ins), _base_port_id(baseID) {}; + + ~UavcanEscController() {}; + + 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; + reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; + + for (uint8_t i = 0; i < num_outputs; i++) { + msg_sp.value[i] = outputs[i]; + } + + uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .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. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _esc_setpoint_transfer_id, + .payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &esc_sp_payload_buffer, + }; + + int result = reg_drone_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, + &transfer.payload_size); + + 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. + result = canardTxPush(&_canard_instance, &transfer); + } + } + }; + + /** + * Sets the number of rotors + */ + void set_rotor_count(uint8_t count) { _rotor_count = count; } + +private: + /** + * ESC status message reception will be reported via this callback. + */ + 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}; +}; diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index cc078df40c..8851264d91 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -65,12 +65,13 @@ private: /// TODO: /// use qsort() to order alphabetically by UAVCAN name /// copy over Ken's parameter find/get/set code - const UavcanParamBinder _uavcan_params[6] { + const UavcanParamBinder _uavcan_params[7] { {"uavcan.pub.esc.0.id", "UCAN1_ESC0_PID"}, {"uavcan.pub.servo.0.id", "UCAN1_SERVO0_PID"}, {"uavcan.sub.gps.0.id", "UCAN1_GPS0_PID"}, {"uavcan.sub.gps.1.id", "UCAN1_GPS1_PID"}, {"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, {"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"}, + {"uavcan.sub.esc.0.id", "UCAN1_ESC_PID"}, }; }; diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/Esc.hpp new file mode 100644 index 0000000000..59c3e2940e --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/Esc.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 Esc.hpp + * + * Defines basic functionality of UAVCAN v1 ESC setpoint subscription + * (For use on a CAN-->PWM node) + * + * @author Jacob Crabill + */ + +#pragma once + +// DS-15 Specification Messages +#include + +#include "Subscriber.hpp" + +class UavcanEscSubscription : public UavcanSubscription +{ +public: + UavcanEscSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanSubscription(ins, pmgr, uavcan_pname) { }; + + void subscribe() override + { + // Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1 + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + _port_id, + reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub); + + /** TODO: Add additional ESC-service messages: reg.drone.service.common.Readiness + */ + }; + + void callback(const CanardTransfer &receive) override + { + // Test with Yakut: + // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" + // yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: {1000, 2000, 3000, 4000}, longitude: 2.34, altitude: {meter: 0.5}}' + PX4_INFO("EscCallback"); + + reg_drone_service_actuator_common_sp_Vector8_0_1 esc {}; + size_t esc_size_in_bits = receive.payload_size; + reg_drone_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload, + &esc_size_in_bits); + + double val1 = static_cast(esc.value[0]); + double val2 = static_cast(esc.value[1]); + double val3 = static_cast(esc.value[2]); + double val4 = static_cast(esc.value[3]); + PX4_INFO("values[0-3] = {%f, %f, %f, %f}", val1, val2, val3, val4); + /// do something with the data + + /// TODO: Publish to output_control_mc + }; + +}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 31084d7139..2191c997d9 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -478,6 +478,8 @@ void UavcanNode::sendHeartbeat() _uavcan_node_heartbeat_last = transfer.timestamp_usec; + /// TESTING -- Remove before flight! + // Since we don't have a mixer file here yet uint16_t outputs[8] {}; outputs[0] = 1000; outputs[1] = 2000; diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index ec2cc57be6..6eb3c5bbb7 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -79,8 +79,9 @@ #include "CanardInterface.hpp" #include "Publishers/Publisher.hpp" #include "Subscribers/Subscriber.hpp" -#include "Subscribers/Gnss.hpp" #include "Subscribers/Battery.hpp" +#include "Subscribers/Esc.hpp" +#include "Subscribers/Gnss.hpp" #include "Actuators/EscClient.hpp" /// TODO: Add EscServer.hpp for node-side service /** @@ -238,8 +239,9 @@ private: UavcanGpsSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"}; UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"}; UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"}; + UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"}; - UavcanSubscription *_subscribers[4] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}; /// TODO: turn into List + UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List UavcanEscController _esc_controller {_canard_instance, 22}; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 9268a79cda..80e09cf858 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -93,3 +93,4 @@ PARAM_DEFINE_INT32(UCAN1_GPS0_PID, 0); PARAM_DEFINE_INT32(UCAN1_GPS1_PID, 0); PARAM_DEFINE_INT32(UCAN1_BMS0_PID, 0); PARAM_DEFINE_INT32(UCAN1_BMS1_PID, 0); +PARAM_DEFINE_INT32(UCAN1_ESC_PID, 0);