fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Clean up handling of invalid port IDs

Browse Source 
Always convert 'invalid' port ID values (e.g., the default PX4 port-ID
parameter value of -1) to CANARD_PORT_ID_UNSET in all comparisons.
This commit is contained in:
JacobCrabill 2021-06-08 15:16:06 -07:00 committed by Daniel Agar
parent 73ef22ae77
commit 0931ff10d5
3 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/drivers/uavcan_v1/Publishers/Publisher.hpp
View File

@ -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;

7
src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
View File

@ -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) {

6
src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp
View File

@ -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) {