diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 48c9cb661d..1a444d7485 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -60,6 +60,7 @@ 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) { }; @@ -67,6 +68,8 @@ public: // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() = 0; + bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; } + CanardPortID id() { return _port_id; }; void updateParam() @@ -79,9 +82,16 @@ public: _param_manager.GetParamByName(uavcan_param, value); int32_t new_id = value.integer32.value.elements[0]; + // Allow, for example, a default PX4 param value of '-1' to disable publication + if (!isValidPortId(new_id)) { + // but always use the standard 'unset' value for comparison + new_id = CANARD_PORT_ID_UNSET; + } + if (_port_id != new_id) { if (new_id == CANARD_PORT_ID_UNSET) { PX4_INFO("Disabling publication of subject %s.%d", _subject_name, _instance); + _port_id = CANARD_PORT_ID_UNSET; } else { _port_id = (CanardPortID)new_id; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp index 3ab7b0dcf3..8ab79daf4d 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp @@ -51,6 +51,7 @@ 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) @@ -64,6 +65,8 @@ public: unsubscribe(); } + bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; } + virtual void subscribe() = 0; virtual void unsubscribe() { @@ -96,6 +99,10 @@ public: bool hasPortID(CanardPortID port_id) { + if (!isValidPortId((int32_t)port_id)) { + return false; + } + SubjectSubscription *curSubj = &_subj_sub; while (curSubj != NULL) { diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp index eb8da7e62e..e6696c2a41 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp @@ -72,6 +72,12 @@ public: if (_param_manager.GetParamByName(uavcan_param, value)) { int32_t new_id = value.integer32.value.elements[0]; + // Allow, for example, a default PX4 param value of '-1' to disable subscription + if (!isValidPortId(new_id)) { + // but always use the standard 'unset' value for comparison + new_id = CANARD_PORT_ID_UNSET; + } + /* FIXME how about partial subscribing */ if (curSubj->_canard_sub.port_id != new_id) { if (new_id == CANARD_PORT_ID_UNSET) {