[UAVCANv1] TX deadline fix and other fixes

.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/

src/drivers/uavcan_v1/Actuators/EscClient.hpp

@@ -102,7 +102,7 @@ public:
 
 				CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_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.

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")

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.

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<CanardPortID>((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.

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.

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.

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.

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.

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.

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;
 			}
 

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) {

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_,

src/drivers/uavcan_v1/legacy_data_types

@@ -0,0 +1 @@
+Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e

src/drivers/uavcan_v1/public_regulated_data_types

@@ -1 +1 @@
-Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e
+Subproject commit 309b251a7e8d713d6bf428e18e28d91d5f07b73e