Cyphal: add port.List

src/drivers/cyphal/Cyphal.cpp

@@ -125,7 +125,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);
 
 	} else {
-		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC);
 	}
 
 	if (_instance == nullptr) {
@@ -188,6 +188,8 @@ void CyphalNode::Run()
 		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 		sendHeartbeat();
 
+		sendPortList();
+
 		// Check all publishers
 		_pub_manager.update();
 
@@ -392,6 +394,45 @@ void CyphalNode::sendHeartbeat()
 	}
 }
 
+void CyphalNode::sendPortList()
+{
+	static hrt_abstime _uavcan_node_port_List_last{0};
+
+	if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) {
+		return;
+	}
+
+	static uavcan_node_port_List_0_1 msg{};
+	static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_];
+	static CanardTransferID _uavcan_node_port_List_transfer_id{0};
+	size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_;
+	const hrt_abstime now = hrt_absolute_time();
+
+	const CanardTransferMetadata transfer_metadata = {
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindMessage,
+		.port_id        = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
+		.remote_node_id = CANARD_NODE_ID_UNSET,
+		.transfer_id    = _uavcan_node_port_List_transfer_id++
+	};
+
+	// memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_);
+	uavcan_node_port_List_0_1_initialize_(&msg);
+
+	_pub_manager.fillSubjectIdList(msg.publishers);
+	_sub_manager.fillSubjectIdList(msg.subscribers);
+
+	uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size);
+
+	_canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+			      &transfer_metadata,
+			      payload_size,
+			      &uavcan_node_port_List_0_1_buffer
+			     );
+
+	_uavcan_node_port_List_last = now;
+}
+
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {

src/drivers/cyphal/Cyphal.hpp

@@ -137,6 +137,9 @@ private:
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();
 
+	// Sends a port.List at 3s intervals
+	void sendPortList();
+
 	px4::atomic_bool _task_should_exit{false};	///< flag to indicate to tear down the CAN driver
 
 	bool _initialized{false};		///< number of actuators currently available

src/drivers/cyphal/PublicationManager.cpp

@@ -125,6 +125,15 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
 	return NULL;
 }
 
+void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list);
+
+	for (auto &dynpub : _dynpublishers) {
+		publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id();
+		publishers_list.sparse_list.count++;
+	}
+}
 
 void PublicationManager::update()
 {

src/drivers/cyphal/PublicationManager.hpp

@@ -79,6 +79,7 @@
 
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uavcan/node/port/List_0_1.h>
 
 #include "Actuators/EscClient.hpp"
 #include "Publishers/udral/Readiness.hpp"
@@ -103,6 +104,7 @@ public:
 
 	UavcanPublisher *getPublisher(const char *subject_name);
 
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list);
 private:
 	void updateDynamicPublications();
 

src/drivers/cyphal/SubscriptionManager.cpp

@@ -158,3 +158,24 @@ void SubscriptionManager::updateParams()
 	// Check for any newly-enabled subscriptions
 	updateDynamicSubscriptions();
 }
+
+void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list);
+
+	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+	auto &sparse_list = subscribers_list.sparse_list;
+
+	while (dynsub != nullptr) {
+		int32_t instance_idx = 0;
+
+		while (dynsub->isValidPortId(dynsub->id(instance_idx))) {
+			sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx);
+			sparse_list.count++;
+			instance_idx++;
+		}
+
+		dynsub = dynsub->next();
+	}
+}

src/drivers/cyphal/SubscriptionManager.hpp

@@ -71,6 +71,7 @@
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
+#include <uavcan/node/port/List_0_1.h>
 #include "Subscribers/DynamicPortSubscriber.hpp"
 #include "CanardInterface.hpp"
 
@@ -100,6 +101,7 @@ public:
 	void subscribe();
 	void printInfo();
 	void updateParams();
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list);
 
 private:
 	void updateDynamicSubscriptions();