From 0c926250a2ca68f363e4b9c7a79627b3cd53c616 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Fri, 7 May 2021 17:54:39 +0200 Subject: [PATCH] UAVCANv1 cleanup and uORB over UAVCANV1 move to own subclass --- .../nuttx-config/socketcan/defconfig | 2 +- src/drivers/uavcan_v1/CanardNuttXCDev.cpp | 2 +- src/drivers/uavcan_v1/CanardSocketCAN.cpp | 4 +- src/drivers/uavcan_v1/NodeManager.cpp | 28 +++- src/drivers/uavcan_v1/NodeManager.hpp | 8 +- src/drivers/uavcan_v1/ParamManager.cpp | 29 +++- src/drivers/uavcan_v1/ParamManager.hpp | 3 +- .../uavcan_v1/Publishers/uORB/sensor_gps.hpp | 84 ++++++++++ .../uavcan_v1/Services/AccessRequest.hpp | 58 ++++--- .../Subscribers/DynamicPortSubscriber.hpp | 32 ++-- .../uavcan_v1/Subscribers/uORB/sensor_gps.hpp | 83 ++++++++++ src/drivers/uavcan_v1/Uavcan.cpp | 156 +++++++++--------- src/drivers/uavcan_v1/Uavcan.hpp | 30 +--- src/drivers/uavcan_v1/parameters.c | 3 + src/drivers/uavcannode_gps_demo/canard_main.c | 2 +- .../libcancl/registerinterface.c | 5 +- 16 files changed, 361 insertions(+), 168 deletions(-) create mode 100644 src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp create mode 100644 src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp diff --git a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig index b0cfe5dbf2..b4c08cc30d 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig @@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_IOB_NBUFFERS=48 -CONFIG_IOB_NCHAINS=16 +CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y CONFIG_KINETIS_CRC=y diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp b/src/drivers/uavcan_v1/CanardNuttXCDev.cpp index f854cbf60e..f9949c58ab 100644 --- a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp +++ b/src/drivers/uavcan_v1/CanardNuttXCDev.cpp @@ -126,7 +126,7 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) // Any recieved CAN messages will cause the poll statement to unblock and run // This way CAN read runs with minimal latency. // Note that multiple messages may be received in a short time, so this will try to read any availible in a loop - ::poll(&fds, 1, 10); + ::poll(&fds, 1, 0); // Only execute this part if can0 is changed. if (fds.revents & POLLIN) { diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/uavcan_v1/CanardSocketCAN.cpp index c26ad2a9ce..85e711fd74 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/uavcan_v1/CanardSocketCAN.cpp @@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms) _send_tv->tv_usec = txf.timestamp_usec % 1000000ULL; _send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL; - return sendmsg(_fd, &_send_msg, 1000); + return sendmsg(_fd, &_send_msg, 0); } int16_t CanardSocketCAN::receive(CanardFrame *rxf) { - int32_t result = recvmsg(_fd, &_recv_msg, 1000); + int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); if (result < 0) { return result; diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp index 1cb34d7ad3..ec841bbf14 100644 --- a/src/drivers/uavcan_v1/NodeManager.cpp +++ b/src/drivers/uavcan_v1/NodeManager.cpp @@ -39,16 +39,20 @@ * @author Peter van der Perk */ +#define RETRY_COUNT 10 + #include "NodeManager.hpp" bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) { if (msg.allocated_node_id.count == 0) { + uint32_t i; + msg.allocated_node_id.count = 1; msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET; /* Search for an available NodeID to assign */ - for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { + for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { if (i == _canard_instance.node_id) { continue; // Don't give our NodeID to a node @@ -63,6 +67,10 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) } } + nodeid_registry[i].register_setup = false; // Re-instantiate register setup + nodeid_registry[i].register_index = 0; + nodeid_registry[i].retry_count = 0; + if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) { PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); @@ -120,17 +128,15 @@ void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_ if (nodeid_registry[i].node_id == node_id) { nodeid_registry[i].register_index++; // Increment index counter for next update() nodeid_registry[i].register_setup = false; + nodeid_registry[i].retry_count = 0; } } + if (_access_request.setPortId(node_id, msg.name)) { + PX4_INFO("Set portID succesfull"); - if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name, - msg.name.name.count) == 0) { - _access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager. - - } else if (strncmp((char *)msg.name.name.elements, bms_status_register_name, - msg.name.name.count) == 0) { //Battery status publisher - _access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager. + } else { + PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements); } } } @@ -142,7 +148,11 @@ void NodeManager::update() if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) { //Setting up registers _list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index); - nodeid_registry[i].register_setup = true; + nodeid_registry[i].retry_count++; + + if (nodeid_registry[i].retry_count > RETRY_COUNT) { + nodeid_registry[i].register_setup = true; // Don't update anymore + } } } diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/uavcan_v1/NodeManager.hpp index 37ae76b0cb..50d31294c3 100644 --- a/src/drivers/uavcan_v1/NodeManager.hpp +++ b/src/drivers/uavcan_v1/NodeManager.hpp @@ -63,12 +63,14 @@ typedef struct { uint8_t unique_id[16]; bool register_setup; uint16_t register_index; + uint16_t retry_count; } UavcanNodeEntry; class NodeManager { public: - NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(ins) { }; + NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr), + _list_request(ins) { }; bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg); bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg); @@ -89,8 +91,4 @@ private: bool nodeRegisterSetup = 0; hrt_abstime _register_request_last{0}; - - //TODO work this out - const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id"; - const char *bms_status_register_name = "uavcan.pub.battery_status.id"; }; diff --git a/src/drivers/uavcan_v1/ParamManager.cpp b/src/drivers/uavcan_v1/ParamManager.cpp index 9c5e34c951..35eb42452b 100644 --- a/src/drivers/uavcan_v1/ParamManager.cpp +++ b/src/drivers/uavcan_v1/ParamManager.cpp @@ -40,6 +40,7 @@ */ #include "ParamManager.hpp" +#include bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value) { @@ -56,8 +57,16 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ case PARAM_TYPE_INT32: { int32_t out_val {}; param_get(param_handle, &out_val); - value.integer32.value.elements[0] = out_val; - uavcan_register_Value_1_0_select_integer32_(&value); + + if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite + value.natural16.value.elements[0] = (uint16_t)out_val; + uavcan_register_Value_1_0_select_natural16_(&value); + + } else { + value.integer32.value.elements[0] = out_val; + uavcan_register_Value_1_0_select_integer32_(&value); + } + break; } @@ -91,15 +100,23 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua switch (param_type(param_handle)) { case PARAM_TYPE_INT32: { int32_t out_val {}; - param_set(param_handle, &out_val); - value.integer32.value.elements[0] = out_val; - uavcan_register_Value_1_0_select_integer32_(&value); + param_get(param_handle, &out_val); + + if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite + value.natural16.value.elements[0] = (uint16_t)out_val; + uavcan_register_Value_1_0_select_natural16_(&value); + + } else { + value.integer32.value.elements[0] = out_val; + uavcan_register_Value_1_0_select_integer32_(&value); + } + break; } case PARAM_TYPE_FLOAT: { float out_val {}; - param_set(param_handle, &out_val); + param_get(param_handle, &out_val); value.real32.value.elements[0] = out_val; uavcan_register_Value_1_0_select_real32_(&value); break; diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 81888b930f..dd6c4745c7 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -63,7 +63,7 @@ public: private: - const UavcanParamBinder _uavcan_params[10] { + const UavcanParamBinder _uavcan_params[11] { {"uavcan.pub.esc.0.id", "UCAN1_ESC_PUB"}, {"uavcan.pub.servo.0.id", "UCAN1_SERVO_PUB"}, {"uavcan.pub.gps.0.id", "UCAN1_GPS_PUB"}, @@ -74,6 +74,7 @@ private: {"uavcan.sub.battery_status.0.id", "UCAN1_BMS_BS_PID"}, {"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"}, {"uavcan.sub.legacy_bms.0.id", "UCAN1_LG_BMS_PID"}, + {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS"}, //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_PID"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_PID"}, }; diff --git a/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp new file mode 100644 index 0000000000..54e0e7de05 --- /dev/null +++ b/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 sensor_gps.hpp + * +* Defines uORB over UAVCANv1 sensor_gps publisher + * + * @author Peter van der Perk + */ + +#pragma once + +#include + +#include "../Publisher.hpp" + +class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher +{ +public: + UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, "sensor_gps", instance) + {}; + + // Update the uORB Subscription and broadcast a UAVCAN message + virtual void update() override + { + // Not sure if actuator_armed is a good indication of readiness but seems close to it + if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) { + sensor_gps_s gps_msg {}; + _sensor_gps_sub.update(&gps_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 = sizeof(struct sensor_gps_s), + .payload = &gps_msg, + }; + + 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); + } + } + }; + +private: + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; +}; diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/uavcan_v1/Services/AccessRequest.hpp index 232e78bbc4..16a62d69f4 100644 --- a/src/drivers/uavcan_v1/Services/AccessRequest.hpp +++ b/src/drivers/uavcan_v1/Services/AccessRequest.hpp @@ -52,45 +52,55 @@ class UavcanAccessServiceRequest { public: - UavcanAccessServiceRequest(CanardInstance &ins) : - _canard_instance(ins) { }; + UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) : + _canard_instance(ins), _param_manager(pmgr) { }; - void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id) + bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name) { int result {0}; uavcan_register_Access_Request_1_0 request_msg; - strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0)); - request_msg.name.name.count = strlen(register_name); - - uavcan_register_Value_1_0_select_natural16_(&request_msg.value); request_msg.value.natural16.value.count = 1; - request_msg.value.natural16.value.elements[0] = port_id; + uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type - uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + //FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis + name.name.elements[7] = 's'; //HACK Change pub into sub - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindRequest, - .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = access_request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, - }; + if (_param_manager.GetParamByName(name, request_msg.value)) { + name.name.elements[7] = 'p'; //HACK Change sub into pub + memcpy(&request_msg.name, &name, sizeof(request_msg.name)); - result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindRequest, + .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = access_request_transfer_id, + .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &request_payload_buffer, + }; + + result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + + return result > 0; + + } else { + return false; } }; private: CanardInstance &_canard_instance; CanardTransferID access_request_transfer_id = 0; + UavcanParamManager &_param_manager; }; diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index dcd379f720..d2e1e2292e 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -68,25 +68,27 @@ 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]; - /* FIXME how about partial subscribing */ - if (curSubj->_canard_sub._port_id != new_id) { - if (new_id == CANARD_PORT_ID_UNSET) { - // Cancel subscription - unsubscribe(); + if (_param_manager.GetParamByName(uavcan_param, value)) { + int32_t new_id = value.integer32.value.elements[0]; - } else { - if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) { - // Already active; unsubscribe first + /* FIXME how about partial subscribing */ + if (curSubj->_canard_sub._port_id != new_id) { + if (new_id == CANARD_PORT_ID_UNSET) { + // Cancel subscription unsubscribe(); - } - // Subscribe on the new port ID - curSubj->_canard_sub._port_id = (CanardPortID)new_id; - PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id); - subscribe(); + } else { + if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) { + // Already active; unsubscribe first + unsubscribe(); + } + + // Subscribe on the new port ID + curSubj->_canard_sub._port_id = (CanardPortID)new_id; + PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id); + subscribe(); + } } } diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp new file mode 100644 index 0000000000..d7ddbfa0ae --- /dev/null +++ b/src/drivers/uavcan_v1/Subscribers/uORB/sensor_gps.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 sensor_gps.hpp + * + * Defines uORB over UAVCANv1 sensor_gps subscriber + * + * @author Peter van der Perk + */ + +#pragma once + +#include +#include + +#include "../DynamicPortSubscriber.hpp" + +class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber +{ +public: + UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { }; + + void subscribe() override + { + // Subscribe to messages uORB sensor_gps payload over UAVCAN + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + _subj_sub._canard_sub._port_id, + sizeof(struct sensor_gps_s), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000, + &_subj_sub._canard_sub); + }; + + void callback(const CanardTransfer &receive) override + { + //PX4_INFO("uORB sensor_gps Callback"); + + if (receive.payload_size == sizeof(struct sensor_gps_s)) { + sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; + _sensor_gps_pub.publish(*gps_msg); + + } else { + PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d", + _subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s)); + } + }; + +private: + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + +}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 058e5a8922..5c1d34fd0b 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -56,8 +56,7 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), - _can_interface(interface)//, - // _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub} + _can_interface(interface) { pthread_mutex_init(&_node_mutex, nullptr); @@ -168,13 +167,6 @@ void UavcanNode::init() CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_heartbeat_subscription); - canardRxSubscribe(&_canard_instance, // uORB over UAVCAN GPS message - CanardTransferKindMessage, - gps_port_id, - sizeof(struct sensor_gps_s), - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_drone_srv_gps_subscription); - for (auto &subscriber : _subscribers) { subscriber->subscribe(); } @@ -238,7 +230,69 @@ void UavcanNode::Run() _node_manager.update(); - // Transmitting + transmit(); + + /* Process received messages */ + + uint8_t data[64] {}; + CanardFrame received_frame{}; + received_frame.payload = &data; + + while (_can_interface->receive(&received_frame) > 0) { + CanardTransfer receive{}; + int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if + // the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + // Reception of an invalid frame is NOT an error. + PX4_ERR("Receive error %d\n", result); + + } else if (result == 1) { + // A transfer has been received, process it. + // PX4_INFO("received Port ID: %d", receive.port_id); + + if (receive.port_id > 0) { + // If not a fixed port ID, check any subscribers which may have registered it + for (auto &subscriber : _subscribers) { + if (subscriber->hasPortID(receive.port_id)) { + subscriber->callback(receive); + } + } + + for (auto &subscriber : _dynsubscribers) { + if (subscriber->hasPortID(receive.port_id)) { + subscriber->callback(receive); + } + } + } + + // Deallocate the dynamic memory afterwards. + _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); + + } else { + //PX4_INFO("RX canard %d", result); + } + } + + // Pop canardTx queue to send out responses to requets + transmit(); + + perf_end(_cycle_perf); + + if (_task_should_exit.load()) { + _can_interface->close(); + + ScheduleClear(); + _instance = nullptr; + } + + pthread_mutex_unlock(&_node_mutex); +} + +void UavcanNode::transmit() +{ // Look at the top of the TX queue. for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue. @@ -268,67 +322,6 @@ void UavcanNode::Run() } } } - - /* Process received messages */ - - uint8_t data[64] {}; - CanardFrame received_frame{}; - received_frame.payload = &data; - - /* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer */ - - if (_can_interface->receive(&received_frame) > 0) { - - CanardTransfer receive{}; - int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive); - - if (result < 0) { - // An error has occurred: either an argument is invalid or we've ran out of memory. - // It is possible to statically prove that an out-of-memory will never occur for a given application if - // the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. - // Reception of an invalid frame is NOT an error. - PX4_ERR("Receive error %d\n", result); - - } else if (result == 1) { - // A transfer has been received, process it. - // PX4_INFO("received Port ID: %d", receive.port_id); - - if (receive.port_id == gps_port_id) { - result = handleUORBSensorGPS(receive); - - } else if (receive.port_id > 0) { - // If not a fixed port ID, check any subscribers which may have registered it - for (auto &subscriber : _subscribers) { - if (subscriber->hasPortID(receive.port_id)) { - subscriber->callback(receive); - } - } - - for (auto &subscriber : _dynsubscribers) { - if (subscriber->hasPortID(receive.port_id)) { - subscriber->callback(receive); - } - } - } - - // Deallocate the dynamic memory afterwards. - _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); - - } else { - //PX4_INFO("RX canard %d\r\n", result); - } - } - - perf_end(_cycle_perf); - - if (_task_should_exit.load()) { - _can_interface->close(); - - ScheduleClear(); - _instance = nullptr; - } - - pthread_mutex_unlock(&_node_mutex); } void UavcanNode::print_info() @@ -338,6 +331,13 @@ void UavcanNode::print_info() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); + O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator); + + PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64, + heap_diagnostics.allocated, heap_diagnostics.capacity, + heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size, + heap_diagnostics.oom_count); + for (auto &publisher : _publishers) { publisher->printInfo(); } @@ -346,6 +346,10 @@ void UavcanNode::print_info() subscriber->printInfo(); } + for (auto &subscriber : _dynsubscribers) { + subscriber->printInfo(); + } + _mixing_output.printInfo(); _esc_controller.printInfo(); @@ -442,16 +446,6 @@ void UavcanNode::sendHeartbeat() } } -int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive) -{ - PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id); - - sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; - - return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1; -} - - bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 9a8f960c7f..2e98e03bc5 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -78,6 +78,9 @@ #include "Subscribers/NodeIDAllocationData.hpp" #include "Subscribers/LegacyBatteryInfo.hpp" +// uORB over UAVCAN subscribers +#include "Subscribers/uORB/sensor_gps.hpp" + #include "ServiceClients/GetInfo.hpp" #include "ServiceClients/Access.hpp" @@ -162,14 +165,11 @@ private: void Run() override; void fill_node_info(); + void transmit(); + // Sends a heartbeat at 1s intervals void sendHeartbeat(); - int handlePnpNodeIDAllocationData(const CanardTransfer &receive); - int handleRegisterList(const CanardTransfer &receive); - int handleRegisterAccess(const CanardTransfer &receive); - int handleUORBSensorGPS(const CanardTransfer &receive); - void *_uavcan_heap{nullptr}; CanardInterface *const _can_interface; @@ -185,14 +185,9 @@ private: pthread_mutex_t _node_mutex; CanardRxSubscription _heartbeat_subscription; - CanardRxSubscription _drone_srv_gps_subscription; - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Publication _battery_status_pub{ORB_ID(battery_status)}; - uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; @@ -201,15 +196,6 @@ private: hrt_abstime _uavcan_node_heartbeat_last{0}; CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; - /* Temporary hardcoded port IDs used by the register interface - * for demo purposes untill we have nice interface (QGC or latter) - * to configure the nodes - */ - const uint16_t gps_port_id = 1235; - - CanardTransferID _uavcan_register_list_request_transfer_id{0}; - CanardTransferID _uavcan_register_access_request_transfer_id{0}; - DEFINE_PARAMETERS( (ParamInt) _param_uavcan_v1_enable, (ParamInt) _param_uavcan_v1_id, @@ -220,7 +206,7 @@ private: UavcanParamManager _param_manager; - NodeManager _node_manager {_canard_instance}; + NodeManager _node_manager {_canard_instance, _param_manager}; UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager}; @@ -238,6 +224,8 @@ private: UavcanLegacyBatteryInfoSubscriber _legacybms_sub {_canard_instance, _param_manager, 0}; UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager}; + UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0}; + UavcanGetInfoResponse _getinfo_rsp {_canard_instance}; UavcanAccessResponse _access_rsp {_canard_instance, _param_manager}; @@ -245,7 +233,7 @@ private: UavcanListServiceReply _list_service {_canard_instance, _node_manager}; // Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message - UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List + UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_sub}; /// TODO: turn into List UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 417fe31de5..f371df84d0 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -103,3 +103,6 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0); PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0); PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0); PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0); + +// uORB over UAVCAN +PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0); diff --git a/src/drivers/uavcannode_gps_demo/canard_main.c b/src/drivers/uavcannode_gps_demo/canard_main.c index a8f62c55db..d63ff39b8c 100644 --- a/src/drivers/uavcannode_gps_demo/canard_main.c +++ b/src/drivers/uavcannode_gps_demo/canard_main.c @@ -381,7 +381,7 @@ static int canard_daemon(int argc, char *argv[]) /* Init UAVCAN register interfaces */ uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO uavcan_register_interface_init(&ins, &node_information); - uavcan_register_interface_add_entry("gnss_uorb", set_gps_uorb_port_id, get_gps_uorb_port_id); + uavcan_register_interface_add_entry("uorb.sensor_gps.0", set_gps_uorb_port_id, get_gps_uorb_port_id); uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id); uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id); diff --git a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c index 299e8dc6fd..241b22080d 100644 --- a/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c +++ b/src/drivers/uavcannode_gps_demo/libcancl/registerinterface.c @@ -238,10 +238,13 @@ int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTrans // Reponse magic start - if (msg.index <= register_list_size) { + if (msg.index < register_list_size) { response_msg.name.name.count = sprintf(response_msg.name.name.elements, "uavcan.pub.%s.id", register_list[msg.index].name); + + } else { + response_msg.name.name.count = 0; } //TODO more option then pub (sub rate