From 74d0f46279065f49f0e94e3c1986802a328d1d2f Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Thu, 24 Jun 2021 17:04:00 -0700 Subject: [PATCH] uavcan_v1: Publisher: Fix port_id update --- .../uavcan_v1/Publishers/Publisher.hpp | 29 +++++++++---------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 1a24fa0c67..3e74e8e7ca 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -68,8 +68,6 @@ 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,23 +77,19 @@ public: // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; - _param_manager.GetParamByName(uavcan_param, value); - uint16_t new_id = value.natural16.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 (_param_manager.GetParamByName(uavcan_param, value)) { + uint16_t new_id = value.natural16.value.elements[0]; - 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; + 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; - PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id); + } else { + _port_id = (CanardPortID)new_id; + PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id); + } } } }; @@ -104,6 +98,9 @@ public: { if (_port_id != CANARD_PORT_ID_UNSET) { PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id); + + } else { + PX4_INFO("Subject %s.%d disabled", _subject_name, _instance); } }