diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 73de1992ff..5ce3a0b77a 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/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) { diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 004a509f40..0132830fb6 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/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 diff --git a/src/drivers/cyphal/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp index 74ef3699c4..84133e0372 100644 --- a/src/drivers/cyphal/PublicationManager.cpp +++ b/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() { diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index 2c3da87d9d..c996e797e9 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -79,6 +79,7 @@ #include #include +#include #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(); diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp index e489357fc2..ef4ae73f2b 100644 --- a/src/drivers/cyphal/SubscriptionManager.cpp +++ b/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(); + } +} diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index bb56e1e057..ce5b12a54d 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -71,6 +71,7 @@ #include #include +#include #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();