diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index 64d4b315ae..5270957cce 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -61,6 +61,7 @@ public: 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) {}; diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 8851264d91..2d3b212ce6 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -62,16 +62,15 @@ public: bool SetParamByName(const uavcan_register_Name_1_0 &name, const uavcan_register_Value_1_0 &value); private: - /// TODO: - /// use qsort() to order alphabetically by UAVCAN name - /// copy over Ken's parameter find/get/set code - const UavcanParamBinder _uavcan_params[7] { - {"uavcan.pub.esc.0.id", "UCAN1_ESC0_PID"}, - {"uavcan.pub.servo.0.id", "UCAN1_SERVO0_PID"}, + + const UavcanParamBinder _uavcan_params[8] { + {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"}, + {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"}, + {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"}, + {"uavcan.sub.esc.0.id", "UCAN1_ESC_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/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/Gnss.hpp new file mode 100644 index 0000000000..b4dbd5ddc5 --- /dev/null +++ b/src/drivers/uavcan_v1/Publishers/Gnss.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * 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 Gnss.hpp + * + * Defines basic functionality of UAVCAN v1 GNSS publisher + * + * @author Jacob Crabill + */ + +#pragma once + +// DS-15 Specification Messages +#include +#include + +#include "Publisher.hpp" + +class UavcanGnssPublication : public UavcanPublication +{ +public: + UavcanGnssPublication(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanPublication(ins, pmgr, uavcan_pname) + { + + }; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() override + { + if (_gps_sub.updated() && _port_id > 0) { + sensor_gps_s gps {}; + _gps_sub.update(&gps); + + reg_drone_physics_kinematics_geodetic_Point_0_1 geo {}; + geo.latitude = gps.lat; + geo.longitude = gps.lon; + geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast(gps.alt) }; + + uint8_t geo_payload_buffer[reg_drone_physics_kinematics_geodetic_Point_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 = _port_id, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + .payload_size = reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &geo_payload_buffer, + }; + + int32_t result = reg_drone_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer, + &transfer.payload_size); + + if (result == 0) { + // 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. + result = canardTxPush(&_canard_instance, &transfer); + } + + /// TODO: Also publish DilutionOfPrecision, ...? + reg_drone_service_gnss_DilutionOfPrecision_0_1 dop { + .geometric = NAN, + .position = NAN, + .horizontal = gps.hdop, + .vertical = gps.vdop, + .time = (float)gps.time_utc_usec / 1.e6f, /// ? + .northing = NAN, + .easting = NAN, + }; + + uint8_t dop_payload_buffer[reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardPortID _port_id_2 = static_cast((uint16_t)_port_id + 1U); + + CanardTransfer transfer2 = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id_2, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id_2, + .payload_size = reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &dop_payload_buffer, + }; + + result = reg_drone_service_gnss_DilutionOfPrecision_0_1_serialize_(&dop, dop_payload_buffer, &transfer2.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_transfer_id_2; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer2); + } + } + }; + +private: + + /// TODO: Allow >1 instance + uORB::Subscription _gps_sub{ORB_ID(sensor_gps)}; + CanardTransferID _transfer_id_2 {0}; +}; diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp new file mode 100644 index 0000000000..2c81ecd464 --- /dev/null +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 Publisher.hpp + * + * Defines basic functionality of UAVCAN v1 publisher class + * + * @author Jacob Crabill + */ + +#pragma once + +#include + +#include + +#include + +#include "../CanardInterface.hpp" +#include "../ParamManager.hpp" + +class UavcanPublication +{ +public: + UavcanPublication(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + _canard_instance(ins), _param_manager(pmgr), _uavcan_param(uavcan_pname) { }; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() = 0; + + CanardPortID id() { return _port_id; }; + + void updateParam() + { + // 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]; + + if (_port_id != new_id) { + if (new_id == 0) { + PX4_INFO("Disabling publication of %s", _uavcan_param); + + } else { + _port_id = (CanardPortID)new_id; + PX4_INFO("Enabling %s on port %d", _uavcan_param, _port_id); + } + } + }; + + void printInfo() + { + if (_port_id > 0) { + PX4_INFO("Enabled %s on port %d", _uavcan_param, _port_id); + } + } + +protected: + CanardInstance &_canard_instance; + UavcanParamManager &_param_manager; + CanardRxSubscription _canard_sub; + const char *_uavcan_param; // Port ID parameter + + CanardPortID _port_id {0}; + CanardTransferID _transfer_id {0}; +}; diff --git a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp index 1d7642a3f0..10d0be33d6 100644 --- a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp @@ -46,10 +46,10 @@ #include "Subscriber.hpp" -class UavcanGpsSubscription : public UavcanSubscription +class UavcanGnssSubscription : public UavcanSubscription { public: - UavcanGpsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : + UavcanGnssSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : UavcanSubscription(ins, pmgr, uavcan_pname) { }; void subscribe() override diff --git a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp index 16558af3de..9fe4fdda34 100644 --- a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp @@ -42,22 +42,10 @@ #pragma once #include -#include -#include -#include -#include #include #include -#include -#include -#include - -//Quick and Dirty PNP imlementation only V1 for now as well -#include -#include -#include #include "../CanardInterface.hpp" #include "../ParamManager.hpp" diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 2191c997d9..4f26508085 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -83,6 +83,10 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : PX4_INFO("main _canard_instance = %p", &_canard_instance); + for (auto &publisher : _publishers) { + publisher->updateParam(); + } + for (auto &subscriber : _subscribers) { subscriber->updateParam(); } @@ -229,6 +233,12 @@ void UavcanNode::Run() // update parameters from storage updateParams(); + for (auto &publisher : _publishers) { + // Have the publisher update its associated port-id parameter + // Setting to 0 disable publication + publisher->updateParam(); + } + for (auto &subscriber : _subscribers) { // Have the subscriber update its associated port-id parameter // If the port-id changes, (re)start the subscription @@ -240,8 +250,6 @@ void UavcanNode::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - - if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s && _node_register_setup != CANARD_NODE_ID_UNSET && _node_register_request_index == _node_register_last_received_index + 1) { @@ -278,6 +286,11 @@ void UavcanNode::Run() // send uavcan::node::Heartbeat_1_0 @ 1 Hz sendHeartbeat(); + // Check all publishers + for (auto &publisher : _publishers) { + publisher->update(); + } + // Transmitting // Look at the top of the TX queue. for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { @@ -381,6 +394,10 @@ void UavcanNode::print_info() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); + for (auto &publisher : _publishers) { + publisher->printInfo(); + } + for (auto &subscriber : _subscribers) { subscriber->printInfo(); } diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index a6471e4805..5575c18898 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -77,11 +77,15 @@ #define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_EXTENT_BYTES_ #include "CanardInterface.hpp" + #include "Publishers/Publisher.hpp" +#include "Publishers/Gnss.hpp" + #include "Subscribers/Subscriber.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 /** @@ -230,15 +234,20 @@ private: ) UavcanParamManager _param_manager; - UavcanGpsSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"}; - 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"}; + + UavcanGnssPublication _gps_pub {_canard_instance, _param_manager, "uavcan.pub.gps.0.id"}; + + UavcanPublication *_publishers[1] {&_gps_pub}; + + UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"}; + UavcanGnssSubscription _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[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List - UavcanEscController _esc_controller {_canard_instance, 22}; + UavcanEscController _esc_controller {_canard_instance, 22}; //// TODO UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; }; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 80e09cf858..6c5335a62d 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -86,11 +86,13 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0); */ PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242); - -PARAM_DEFINE_INT32(UCAN1_ESC0_PID, 0); -PARAM_DEFINE_INT32(UCAN1_SERVO0_PID, 0); +// Subscription Port IDs 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); + +// Publication Port IDs +PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0); +PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0); +PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);