Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 01:17:51 +08:00
Code Issues Actions 5 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
+13 -16
View File
@@ -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);
}
}