Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

uavcan_v1: Publisher: Fix port_id update

Browse Source
This commit is contained in:
JacobCrabill
2021-06-24 17:04:00 -07:00
committed by Daniel Agar
parent 1b5e65df04
commit 74d0f46279
1 changed files with 13 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Publishers/Publisher.hpp
+6 -9
View File
@@ -68,8 +68,6 @@ public:
// Update the uORB Subscription and broadcast a UAVCAN message // Update the uORB Subscription and broadcast a UAVCAN message
virtual void update() = 0; virtual void update() = 0;
bool isValidPortId(int32_t id) const { return id >= 0 && id <= CANARD_PORT_ID_MAX; }
CanardPortID id() { return _port_id; }; CanardPortID id() { return _port_id; };
void updateParam() void updateParam()
@@ -79,14 +77,9 @@ public:
// Set _port_id from _uavcan_param // Set _port_id from _uavcan_param
uavcan_register_Value_1_0 value; 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 (_param_manager.GetParamByName(uavcan_param, value)) {
if (!isValidPortId(new_id)) { uint16_t new_id = value.natural16.value.elements[0];
// but always use the standard 'unset' value for comparison
new_id = CANARD_PORT_ID_UNSET;
}
if (_port_id != new_id) { if (_port_id != new_id) {
if (new_id == CANARD_PORT_ID_UNSET) { if (new_id == CANARD_PORT_ID_UNSET) {
@@ -98,12 +91,16 @@ public:
PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id); PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id);
} }
} }
}
}; };
void printInfo() void printInfo()
{ {
if (_port_id != CANARD_PORT_ID_UNSET) { if (_port_id != CANARD_PORT_ID_UNSET) {
PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id); PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id);
} else {
PX4_INFO("Subject %s.%d disabled", _subject_name, _instance);
} }
} }