diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 3e74e8e7ca..6c6e3653ac 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -59,9 +59,6 @@ class UavcanPublisher { public: - static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; - static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U; - UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) : _canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { }; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index 6d9d090698..debec5e116 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -46,13 +46,11 @@ #include #include "../CanardInterface.hpp" +#include "../ParamManager.hpp" class UavcanBaseSubscriber { public: - static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; - static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U; - UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) : _canard_instance(ins), _instance(instance) { diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp index d9f6149449..1d9a9eb217 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp @@ -42,6 +42,8 @@ #include "SubscriptionManager.hpp" +#include "ParamManager.hpp" + SubscriptionManager::~SubscriptionManager() { UavcanDynamicPortSubscriber *dynsub; @@ -66,12 +68,6 @@ void SubscriptionManager::subscribe() void SubscriptionManager::updateDynamicSubscriptions() { for (auto &sub : _uavcan_subs) { - param_t param_handle = param_find(sub.px4_name); - - if (param_handle == PARAM_INVALID) { - PX4_ERR("Param %s not found", sub.px4_name); - break; - } bool found_subscriber = false; UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; @@ -93,11 +89,14 @@ void SubscriptionManager::updateDynamicSubscriptions() continue; } - if ((param_type(param_handle) == PARAM_TYPE_INT32)) { - int32_t port_id {}; - param_get(param_handle, &port_id); + char uavcan_param[90]; + snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", sub.subject_name, sub.instance); + uavcan_register_Value_1_0 value; - if (port_id >= 0) { // PortID is set create a subscriber + if (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t port_id = value.natural16.value.elements[0]; + + if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber dynsub = sub.create_sub(_canard_instance, _param_manager); if (dynsub == NULL) { @@ -122,6 +121,10 @@ void SubscriptionManager::updateDynamicSubscriptions() dynsub->updateParam(); } + + } else { + PX4_ERR("Port ID param for subscriber %s.%u not found", sub.subject_name, sub.instance); + return; } } } diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp index 26967866de..4073b71da0 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp @@ -58,7 +58,6 @@ #include "Subscribers/uORB/sensor_gps.hpp" typedef struct { - const char *px4_name; UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; const char *subject_name; const uint8_t instance; @@ -91,7 +90,6 @@ private: const UavcanDynSubBinder _uavcan_subs[6] { { - "UCAN1_ESC0_SUB", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanEscSubscriber(ins, pmgr, 0); @@ -100,7 +98,6 @@ private: 0 }, { - "UCAN1_GPS0_SUB", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanGnssSubscriber(ins, pmgr, 0); @@ -109,7 +106,6 @@ private: 0 }, { - "UCAN1_GPS1_SUB", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanGnssSubscriber(ins, pmgr, 1); @@ -118,7 +114,6 @@ private: 1 }, { - "UCAN1_BMS_ES_SUB", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanBmsSubscriber(ins, pmgr, 0); @@ -127,7 +122,6 @@ private: 0 }, { - "UCAN1_LG_BMS_SUB", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0); @@ -136,7 +130,6 @@ private: 0 }, { - "UCAN1_UORB_GPS", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0); diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index c3b8291fb1..6c6d4a7e38 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -80,8 +80,6 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : _canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC; } - PX4_INFO("main _canard_instance = %p", &_canard_instance); - _node_manager.subscribe(); for (auto &publisher : _publishers) {