diff --git a/platforms/common/uORB/ORBSet.hpp b/platforms/common/uORB/ORBSet.hpp index aed38dbc8f6..28e283c7b9e 100644 --- a/platforms/common/uORB/ORBSet.hpp +++ b/platforms/common/uORB/ORBSet.hpp @@ -37,14 +37,12 @@ class ORBSet { public: struct Node { - struct Node *next; - const char *node_name; + Node *next{nullptr}; + const char *node_name{nullptr}; }; - ORBSet() : - _top(nullptr), - _end(nullptr) - { } + ORBSet() = default; + ~ORBSet() { while (_top != nullptr) { @@ -52,11 +50,14 @@ public: if (_top->next == nullptr) { free((void *)_top->node_name); - free(_top); + _top->node_name = nullptr; + + delete _top; _top = nullptr; } } } + void insert(const char *node_name) { Node **p; @@ -68,7 +69,7 @@ public: p = &_end->next; } - *p = (Node *)malloc(sizeof(Node)); + *p = new Node(); if (_end) { _end = _end->next; @@ -102,8 +103,13 @@ public: if (_top && (strcmp(_top->node_name, node_name) == 0)) { p = _top->next; - free((void *)_top->node_name); - free(_top); + + if (_top->node_name) { + free((void *)_top->node_name); + _top->node_name = nullptr; + } + + delete _top; _top = p; if (_top == nullptr) { @@ -135,11 +141,17 @@ private: } a->next = b->next; - free((void *)b->node_name); - free(b); + + if (b->node_name) { + free((void *)b->node_name); + b->node_name = nullptr; + } + + delete b; + b = nullptr; } } - Node *_top; - Node *_end; + Node *_top{nullptr}; + Node *_end{nullptr}; }; diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 0634c4e253f..1dce82b3196 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -120,7 +120,12 @@ int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) orbiocdevexists_t *data = (orbiocdevexists_t *)arg; if (data->check_advertised) { - data->ret = uORB::Manager::orb_exists(get_orb_meta(data->orb_id), data->instance); + if (uORB::Manager::get_instance()) { + data->ret = uORB::Manager::get_instance()->orb_exists(get_orb_meta(data->orb_id), data->instance); + + } else { + data->ret = PX4_ERROR; + } } else { data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; @@ -130,7 +135,7 @@ int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) case ORBIOCDEVADVERTISE: { orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); if (dev) { data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); @@ -230,6 +235,10 @@ int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) { + if (meta == nullptr) { + return PX4_ERROR; + } + int ret = PX4_ERROR; // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) @@ -237,7 +246,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) return ret; } - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); + uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); if (dev) { uORB::DeviceNode *node = dev->getDeviceNode(meta, instance); @@ -266,8 +275,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) ret = px4_access(path, F_OK); - if (ret == -1 && meta != nullptr && !_remote_topics.empty()) { - ret = (_remote_topics.find(meta->o_name) != _remote_topics.end()) ? OK : PX4_ERROR; + if (ret == -1) { + if (_remote_topics.find(meta->o_name)) { + ret = 0; + } } if (ret == 0) { @@ -709,12 +720,9 @@ int16_t uORB::Manager::process_received_message(const char *messageName, int32_t bool uORB::Manager::is_remote_subscriber_present(const char *messageName) { -#ifdef __PX4_NUTTX return _remote_subscriber_topics.find(messageName); -#else - return (_remote_subscriber_topics.find(messageName) != _remote_subscriber_topics.end()); -#endif } + #endif /* ORB_COMMUNICATOR */ #ifdef ORB_USE_PUBLISHER_RULES diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 3546adc4de5..1fbc779da1e 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -41,15 +41,8 @@ #include // For ORB_ID enum #include -#ifdef __PX4_NUTTX -#include "ORBSet.hpp" -#else -#include -#include -#define ORBSet std::set -#endif - #ifdef ORB_COMMUNICATOR +#include "ORBSet.hpp" #include "uORBCommunicator.hpp" #endif /* ORB_COMMUNICATOR */