From 1804cea92c91397cf70160d533eb3119b0961d02 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 20 Apr 2021 13:34:16 +0200 Subject: [PATCH] [UAVCANv1] TX deadline fix and other fixes --- .gitmodules | 5 ++++- src/drivers/uavcan_v1/Actuators/EscClient.hpp | 4 ++-- src/drivers/uavcan_v1/CMakeLists.txt | 3 ++- src/drivers/uavcan_v1/NodeManager.cpp | 2 +- src/drivers/uavcan_v1/Publishers/Gnss.hpp | 4 ++-- src/drivers/uavcan_v1/Publishers/Readiness.hpp | 2 +- src/drivers/uavcan_v1/ServiceClients/Access.hpp | 2 +- src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp | 4 +--- src/drivers/uavcan_v1/Services/AccessRequest.hpp | 2 +- src/drivers/uavcan_v1/Services/ListRequest.hpp | 2 +- src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp | 4 ++-- .../uavcan_v1/Subscribers/DynamicPortSubscriber.hpp | 2 +- src/drivers/uavcan_v1/Uavcan.cpp | 9 +++++---- src/drivers/uavcan_v1/legacy_data_types | 1 + src/drivers/uavcan_v1/public_regulated_data_types | 2 +- 15 files changed, 26 insertions(+), 22 deletions(-) create mode 160000 src/drivers/uavcan_v1/legacy_data_types diff --git a/.gitmodules b/.gitmodules index aef1b8c555..5fd30b97f3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -56,10 +56,13 @@ url = https://github.com/UAVCAN/libcanard.git [submodule "src/drivers/uavcan_v1/public_regulated_data_types"] path = src/drivers/uavcan_v1/public_regulated_data_types - url = https://github.com/PX4/public_regulated_data_types.git + url = https://github.com/UAVCAN/public_regulated_data_types.git [submodule "src/drivers/uavcannode_gps_demo/public_regulated_data_types"] path = src/drivers/uavcannode_gps_demo/public_regulated_data_types url = https://github.com/UAVCAN/public_regulated_data_types.git [submodule "src/drivers/uavcannode_gps_demo/libcanard"] path = src/drivers/uavcannode_gps_demo/libcanard url = https://github.com/UAVCAN/libcanard.git +[submodule "src/drivers/uavcan_v1/legacy_data_types"] + path = src/drivers/uavcan_v1/legacy_data_types + url = https://github.com/px4/public_regulated_data_types/ diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index e680d9ee68..280588da8a 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -102,7 +102,7 @@ public: CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = arming_pid, // This is the subject-ID. @@ -145,7 +145,7 @@ public: 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. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index b9d609c6b4..7a77e4b8f1 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -33,6 +33,7 @@ set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard) set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types) +set(LEGACY_DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/legacy_data_types) px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR}) px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) @@ -40,7 +41,7 @@ px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) find_program(NNVG_PATH nnvg) if(NNVG_PATH) execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg) - execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/legacy) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${LEGACY_DSDL_DIR}/uavcan ${LEGACY_DSDL_DIR}/legacy) execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan) else() message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/uavcan_v1/NodeManager.cpp index f337935aed..3b90f75b89 100644 --- a/src/drivers/uavcan_v1/NodeManager.cpp +++ b/src/drivers/uavcan_v1/NodeManager.cpp @@ -70,7 +70,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/Gnss.hpp index eea856f549..aebaff808f 100644 --- a/src/drivers/uavcan_v1/Publishers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/Gnss.hpp @@ -72,7 +72,7 @@ public: 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. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. @@ -107,7 +107,7 @@ public: 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. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id_2, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/Publishers/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/Readiness.hpp index 624032a825..c3a72d7b17 100644 --- a/src/drivers/uavcan_v1/Publishers/Readiness.hpp +++ b/src/drivers/uavcan_v1/Publishers/Readiness.hpp @@ -76,7 +76,7 @@ public: uint8_t readiness_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/uavcan_v1/ServiceClients/Access.hpp index 372394eb4d..069eb46dc9 100644 --- a/src/drivers/uavcan_v1/ServiceClients/Access.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/Access.hpp @@ -103,7 +103,7 @@ public: uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindResponse, .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp index 11a9420891..b972d87562 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp @@ -107,10 +107,8 @@ public: uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardMicrosecond transmission_deadline = hrt_absolute_time() + 1000 * 100; - CanardTransfer response = { - .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindResponse, .port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/uavcan_v1/Services/AccessRequest.hpp index 15b1789c31..7c92522300 100644 --- a/src/drivers/uavcan_v1/Services/AccessRequest.hpp +++ b/src/drivers/uavcan_v1/Services/AccessRequest.hpp @@ -68,7 +68,7 @@ public: uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindRequest, .port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/Services/ListRequest.hpp b/src/drivers/uavcan_v1/Services/ListRequest.hpp index 8a529cdc1c..dc01d4a587 100644 --- a/src/drivers/uavcan_v1/Services/ListRequest.hpp +++ b/src/drivers/uavcan_v1/Services/ListRequest.hpp @@ -62,7 +62,7 @@ public: uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardTransfer request = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindRequest, .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index 4a536f8c05..d58d91040c 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -71,13 +71,13 @@ public: virtual void callback(const CanardTransfer &msg) = 0; - CanardPortID id(uint32_t id = 0) + CanardPortID id(uint32_t instance = 0) { uint32_t i = 0; SubjectSubscription *curSubj = &_subj_sub; while (curSubj != NULL) { - if (id == i) { + if (instance == i) { return curSubj->_canard_sub._port_id; } diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index dcd379f720..e08a438216 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -69,7 +69,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]; /* FIXME how about partial subscribing */ if (curSubj->_canard_sub._port_id != new_id) { diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 8a64e9796c..4cb405fcce 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -168,7 +168,7 @@ void UavcanNode::init() CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_heartbeat_subscription); - canardRxSubscribe(&_canard_instance, //Temporory GPS message DSDL not defined yet + canardRxSubscribe(&_canard_instance, // uORB over UAVCAN GPS message CanardTransferKindMessage, gps_port_id, sizeof(struct sensor_gps_s), @@ -241,8 +241,9 @@ void UavcanNode::Run() // Transmitting // Look at the top of the TX queue. for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { - // Check if the frame has timed out. - if (hrt_absolute_time() > txf->timestamp_usec) { //FIXME wrong I think + // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue. + // Otherwise just drop it and move on to the next one. + if (txf->timestamp_usec == 0 || hrt_absolute_time() > txf->timestamp_usec) { // Send the frame. Redundant interfaces may be used here. const int tx_res = _can_interface->transmit(*txf); @@ -416,7 +417,7 @@ void UavcanNode::sendHeartbeat() CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), + .timestamp_usec = hrt_absolute_time() + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, diff --git a/src/drivers/uavcan_v1/legacy_data_types b/src/drivers/uavcan_v1/legacy_data_types new file mode 160000 index 0000000000..36a01e428b --- /dev/null +++ b/src/drivers/uavcan_v1/legacy_data_types @@ -0,0 +1 @@ +Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types index 36a01e428b..309b251a7e 160000 --- a/src/drivers/uavcan_v1/public_regulated_data_types +++ b/src/drivers/uavcan_v1/public_regulated_data_types @@ -1 +1 @@ -Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e +Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e