diff --git a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk index 143b15536c..3c2898cb0c 100644 --- a/makefiles/nuttx/toolchain_gnu-arm-eabi.mk +++ b/makefiles/nuttx/toolchain_gnu-arm-eabi.mk @@ -125,7 +125,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # note - requires corresponding support in NuttX INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) -LIBSTDCXX := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libstdc++.a) LIBC := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a) # Language-specific flags @@ -282,8 +281,7 @@ endef define LINK @$(ECHO) "LINK: $1" @$(MKDIR) -p $(dir $1) - $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group - #$(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) ${LIBSTDCXX} $(LIBGCC) --end-group + $(Q) $(LD) $(LDFLAGS) -Map $1.map -o $1 --start-group $2 $(LIBS) $(EXTRA_LIBS) $(LIBGCC) --end-group endef # Convert $1 from a linked object to a raw binary in $2 diff --git a/src/modules/uORB/ORBSet.h b/src/modules/uORB/ORBSet.h new file mode 100644 index 0000000000..2d5c59e06d --- /dev/null +++ b/src/modules/uORB/ORBSet.h @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Mark Charlebois. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +class ORBSet +{ +public: + struct Node { + struct Node *next; + const char * node_name; + }; + + ORBSet() : + _top(nullptr), + _end(nullptr) + { } + ~ORBSet() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + } + } + } + void insert(const char *node_name) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + bool erase(const char *node_name) + { + Node *p = _top; + if (_top && (strcmp(_top->node_name, node_name) == 0)) { + p = _top->next; + free((void *)_top->node_name); + free(_top); + _top = p; + if (_top == nullptr) { + _end = nullptr; + } + return true; + } + while (p->next) { + if (strcmp(p->next->node_name, node_name) == 0) { + unlinkNext(p); + } + } + return nullptr; + } + +private: + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + + Node *_top; + Node *_end; +}; + diff --git a/src/modules/uORB/uORBCommunicator.hpp b/src/modules/uORB/uORBCommunicator.hpp index ad407bd7d8..de6984d1ac 100644 --- a/src/modules/uORB/uORBCommunicator.hpp +++ b/src/modules/uORB/uORBCommunicator.hpp @@ -70,7 +70,7 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + virtual int16_t add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; /** @@ -84,7 +84,7 @@ public: * Note: This does not necessarily mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t remove_subscription( const std::string& messageName ) = 0; + virtual int16_t remove_subscription( const char *messageName ) = 0; /** * Register Message Handler. This is internal for the IChannel implementer* @@ -110,7 +110,7 @@ public: * Note: This does not mean that the receiver as received it. * otherwise = failure. */ - virtual int16_t send_message( const std::string& messageName, int32_t length, uint8_t* data) = 0; + virtual int16_t send_message( const char *messageName, int32_t length, uint8_t* data) = 0; }; /** @@ -133,7 +133,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_add_subscription( const std::string& messageName, int32_t msgRateInHz ) = 0; + virtual int16_t process_add_subscription( const char *messageName, int32_t msgRateInHz ) = 0; /** * Interface to process a received control msg to remove subscription @@ -145,7 +145,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_remove_subscription( const std::string& messageName ) = 0; + virtual int16_t process_remove_subscription( const char *messageName ) = 0; /** * Interface to process the received data message. @@ -161,7 +161,7 @@ public: * handler. * otherwise = failure. */ - virtual int16_t process_received_message( const std::string& messageName, int32_t length, uint8_t* data ) = 0; + virtual int16_t process_received_message( const char *messageName, int32_t length, uint8_t* data ) = 0; }; #endif /* _uORBCommunicator_hpp_ */ diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 319f724a67..bebb35f989 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -43,7 +43,7 @@ #include "uORBCommunicator.hpp" #include -std::map uORB::DeviceMaster::_node_map; +uORB::ORBMap uORB::DeviceMaster::_node_map; uORB::DeviceNode::DeviceNode ( @@ -608,7 +608,7 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) else { // add to the node map;. - _node_map[std::string(nodepath)] = node; + _node_map.insert(nodepath, node); } group_tries++; @@ -631,12 +631,12 @@ uORB::DeviceMaster::ioctl(struct file *filp, int cmd, unsigned long arg) } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) { uORB::DeviceNode* rc = nullptr; - if( _node_map.find( nodepath ) != _node_map.end() ) + if( _node_map.find( nodepath ) ) { - rc = _node_map[nodepath]; + rc = _node_map.get(nodepath); } return rc; } diff --git a/src/modules/uORB/uORBDevices_nuttx.hpp b/src/modules/uORB/uORBDevices_nuttx.hpp index e26598531a..ef4bb3b672 100644 --- a/src/modules/uORB/uORBDevices_nuttx.hpp +++ b/src/modules/uORB/uORBDevices_nuttx.hpp @@ -35,8 +35,8 @@ #define _uORBDevices_nuttx_hpp_ #include -#include -#include +#include +#include #include "uORBCommon.hpp" @@ -44,8 +44,93 @@ namespace uORB { class DeviceNode; class DeviceMaster; + class ORBMap; } +class uORB::ORBMap +{ +public: + struct Node { + struct Node *next; + const char * node_name; + uORB::DeviceNode *node; + }; + + ORBMap() : + _top(nullptr), + _end(nullptr) + { } + ~ORBMap() { + while (_top != nullptr) { + unlinkNext(_top); + if (_top->next == nullptr) { + free((void *)_top->node_name); + free(_top); + _top = nullptr; + _end = nullptr; + } + } + } + void insert(const char *node_name, uORB::DeviceNode*node) + { + Node **p; + if (_top == nullptr) + p = &_top; + else + p = &_end->next; + + *p = (Node *)malloc(sizeof(Node)); + if (_end) + _end = _end->next; + else { + _end = _top; + } + _end->next = nullptr; + _end->node_name = strdup(node_name); + _end->node = node; + } + + bool find(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return true; + } + p = p->next; + } + return false; + } + + uORB::DeviceNode* get(const char *node_name) + { + Node *p = _top; + while (p) { + if (strcmp(p->node_name, node_name) == 0) { + return p->node; + } + } + return nullptr; + } + + void unlinkNext(Node *a) + { + Node *b = a->next; + if (b != nullptr) { + if (_end == b) { + _end = a; + } + a->next = b->next; + free((void *)b->node_name); + free(b); + } + } + +private: + Node *_top; + Node *_end; +}; + /** * Per-object device instance. */ @@ -226,11 +311,11 @@ public: DeviceMaster(Flavor f); virtual ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); + static uORB::DeviceNode* GetDeviceNode( const char * node_name ); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); private: Flavor _flavor; - static std::map _node_map; + static ORBMap _node_map; }; diff --git a/src/modules/uORB/uORBDevices_posix.cpp b/src/modules/uORB/uORBDevices_posix.cpp index ddbce3ff55..95a2374611 100644 --- a/src/modules/uORB/uORBDevices_posix.cpp +++ b/src/modules/uORB/uORBDevices_posix.cpp @@ -652,12 +652,13 @@ uORB::DeviceMaster::ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const std::string& nodepath ) +uORB::DeviceNode* uORB::DeviceMaster::GetDeviceNode( const char *nodepath ) { uORB::DeviceNode* rc = nullptr; - if( _node_map.find( nodepath ) != _node_map.end() ) + std::string np(nodepath); + if( _node_map.find( np ) != _node_map.end() ) { - rc = _node_map[nodepath]; + rc = _node_map[np]; } return rc; } diff --git a/src/modules/uORB/uORBDevices_posix.hpp b/src/modules/uORB/uORBDevices_posix.hpp index 11e333d83f..4fe4d499bd 100644 --- a/src/modules/uORB/uORBDevices_posix.hpp +++ b/src/modules/uORB/uORBDevices_posix.hpp @@ -159,7 +159,7 @@ public: DeviceMaster(Flavor f); ~DeviceMaster(); - static uORB::DeviceNode* GetDeviceNode( const std::string& node_name ); + static uORB::DeviceNode* GetDeviceNode( const char *node_name ); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); private: diff --git a/src/modules/uORB/uORBManager.hpp b/src/modules/uORB/uORBManager.hpp index 89b9d7c13f..60a0cc5541 100644 --- a/src/modules/uORB/uORBManager.hpp +++ b/src/modules/uORB/uORBManager.hpp @@ -36,10 +36,14 @@ #include "uORBCommon.hpp" #include "uORBDevices.hpp" -#include -#include #include +#ifdef __PX4_NUTTX +#include "ORBSet.h" +#else +#include #include +#define ORBSet std::set +#endif #include "uORBCommunicator.hpp" @@ -306,7 +310,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * Utility method to check if there is a remote subscriber present * for a given topic */ - bool is_remote_subscriber_present( const std::string& messageName ); + bool is_remote_subscriber_present( const char * messageName ); private: // class methods /** @@ -345,7 +349,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler static Manager _Instance; // the communicator channel instance. uORBCommunicator::IChannel* _comm_channel; - std::set _remote_subscriber_topics; + ORBSet _remote_subscriber_topics; private: //class methods Manager(); @@ -362,7 +366,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_add_subscription(const std::string& messageName, + virtual int16_t process_add_subscription(const char * messageName, int32_t msgRateInHz); /** @@ -375,7 +379,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_remove_subscription(const std::string& messageName); + virtual int16_t process_remove_subscription(const char * messageName); /** * Interface to process the received data message. @@ -391,7 +395,7 @@ class uORB::Manager : public uORBCommunicator::IChannelRxHandler * handler. * otherwise = failure. */ - virtual int16_t process_received_message(const std::string& messageName, + virtual int16_t process_received_message(const char * messageName, int32_t length, uint8_t* data); }; diff --git a/src/modules/uORB/uORBManager_nuttx.cpp b/src/modules/uORB/uORBManager_nuttx.cpp index 3a188c1646..7b1bcf33d1 100644 --- a/src/modules/uORB/uORBManager_nuttx.cpp +++ b/src/modules/uORB/uORBManager_nuttx.cpp @@ -298,11 +298,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_add_subscription(const std::string& messageName, +int16_t uORB::Manager::process_add_subscription(const char * messageName, int32_t msgRateInHz) { warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = 0; _remote_subscriber_topics.insert( messageName ); char nodepath[orb_maxpath]; @@ -312,7 +312,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); if ( node == nullptr) { warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); } else{ // node is present. @@ -327,10 +327,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::Manager::process_remove_subscription( - const std::string& messageName) + const char * messageName) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = -1; _remote_subscriber_topics.erase( messageName ); char nodepath[orb_maxpath]; @@ -340,7 +340,7 @@ int16_t uORB::Manager::process_remove_subscription( // get the node name. if ( node == nullptr) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName.c_str()); + __LINE__, messageName); } else { // node is present. node->process_remove_subscription(); @@ -352,10 +352,10 @@ int16_t uORB::Manager::process_remove_subscription( //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_received_message(const std::string& messageName, +int16_t uORB::Manager::process_received_message(const char * messageName, int32_t length, uint8_t* data) { - //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); int16_t rc = -1; char nodepath[orb_maxpath]; @@ -365,7 +365,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, // get the node name. if ( node == nullptr) { warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", - __LINE__, messageName.c_str(), nodepath ); + __LINE__, messageName, nodepath ); } else { // node is present. @@ -376,7 +376,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, return rc; } -bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) { - return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); + return _remote_subscriber_topics.find( messageName ); } diff --git a/src/modules/uORB/uORBManager_posix.cpp b/src/modules/uORB/uORBManager_posix.cpp index 1954de2784..7cd0d68e9f 100644 --- a/src/modules/uORB/uORBManager_posix.cpp +++ b/src/modules/uORB/uORBManager_posix.cpp @@ -312,11 +312,11 @@ uORBCommunicator::IChannel* uORB::Manager::get_uorb_communicator( void ) //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_add_subscription(const std::string& messageName, +int16_t uORB::Manager::process_add_subscription(const char *messageName, int32_t msgRateInHz) { warnx("[posix-uORB::Manager::process_add_subscription(%d)] entering Manager_process_add_subscription: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = 0; _remote_subscriber_topics.insert( messageName ); char nodepath[orb_maxpath]; @@ -326,7 +326,7 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, uORB::DeviceNode* node = uORB::DeviceMaster::GetDeviceNode( nodepath ); if ( node == nullptr) { warnx( "[posix-uORB::Manager::process_add_subscription(%d)]DeviceNode(%s) not created yet", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); } else{ // node is present. @@ -341,10 +341,10 @@ int16_t uORB::Manager::process_add_subscription(const std::string& messageName, //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int16_t uORB::Manager::process_remove_subscription( - const std::string& messageName) + const char * messageName) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)] Enter: name: %s", - __LINE__, messageName.c_str() ); + __LINE__, messageName ); int16_t rc = -1; _remote_subscriber_topics.erase( messageName ); char nodepath[orb_maxpath]; @@ -354,7 +354,7 @@ int16_t uORB::Manager::process_remove_subscription( // get the node name. if ( node == nullptr) { warnx("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName.c_str()); + __LINE__, messageName); } else { // node is present. node->process_remove_subscription(); @@ -366,10 +366,10 @@ int16_t uORB::Manager::process_remove_subscription( //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- -int16_t uORB::Manager::process_received_message(const std::string& messageName, +int16_t uORB::Manager::process_received_message(const char * messageName, int32_t length, uint8_t* data) { - //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName.c_str() ); + //warnx("[uORB::Manager::process_received_message(%d)] Enter name: %s", __LINE__, messageName ); int16_t rc = -1; char nodepath[orb_maxpath]; @@ -379,7 +379,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, // get the node name. if ( node == nullptr) { warnx("[uORB::Manager::process_received_message(%d)]Error No existing subscriber found for message: [%s] nodepath:[%s]", - __LINE__, messageName.c_str(), nodepath ); + __LINE__, messageName, nodepath ); } else { // node is present. @@ -390,7 +390,7 @@ int16_t uORB::Manager::process_received_message(const std::string& messageName, return rc; } -bool uORB::Manager::is_remote_subscriber_present( const std::string& messageName ) +bool uORB::Manager::is_remote_subscriber_present( const char * messageName ) { return ( _remote_subscriber_topics.find( messageName ) != _remote_subscriber_topics.end() ); } diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index 3bedc5ecd9..a18fa1453a 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -65,14 +65,14 @@ int uORB::Utils::node_mkpath //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int uORB::Utils::node_mkpath(char *buf, Flavor f, - const std::string& orbMsgName ) + const char *orbMsgName ) { unsigned len; unsigned index = 0; len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", - orbMsgName.c_str(), index ); + orbMsgName, index ); if (len >= orb_maxpath) return -ENAMETOOLONG; diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index f9df35eee8..522d255b26 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -55,7 +55,7 @@ public: /** * same as above except this generators the path based on the string. */ - static int node_mkpath(char *buf, Flavor f, const std::string& orbMsgName); + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); };