diff options
-rw-r--r-- | nuttx-configs/px4fmu-v2/nsh/Make.defs | 4 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber.cpp | 10 | ||||
-rw-r--r-- | src/platforms/nuttx/px4_nodehandle.cpp | 19 | ||||
-rw-r--r-- | src/platforms/nuttx/px4_nuttx_impl.cpp | 12 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 13 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 42 |
6 files changed, 74 insertions, 26 deletions
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index f3ce53b4a..5ae27aa00 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -90,7 +90,7 @@ else ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) else - # Linux/Cygwin-native toolchain + # Linux/Cygwin-native toolchain MKDEP = $(TOPDIR)/tools/mkdeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx @@ -117,7 +117,7 @@ ARCHOPTIMIZATION += -g endif ARCHCFLAGS = -std=gnu99 -ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=c++11 ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 0d0a81d7e..f8b138844 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -36,6 +36,10 @@ void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%lu]", msg.timestamp_last_valid); } +// void rc_channels_callback(int i) +// { + // PX4_INFO("I heard: [%d]", i); +// } namespace px4 { bool task_should_exit = false; @@ -77,7 +81,11 @@ PX4_MAIN_FUNCTION(subscriber) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); + // n.subscribe(PX4_TOPIC(rc_channels), [&](const PX4_TOPIC_T(rc_channels) &msg){rc_channels_callback(msg);}); + // n.subscribe(PX4_TOPIC(rc_channels), [&](int i){ return rc_channels_callback(i);}); + // CallbackFunction cbf = [](int i){ return rc_channels_callback(i);}; + std::function<void(const PX4_TOPIC_T(rc_channels)&)> cbf = [](const PX4_TOPIC_T(rc_channels)& msg){ return rc_channels_callback(msg);}; + n.subscribe<PX4_TOPIC_T(rc_channels)>(PX4_TOPIC(rc_channels), cbf); PX4_INFO("subscribed"); /** diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 091b5c6c6..7930a0680 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -36,9 +36,28 @@ * * PX4 Middleware Wrapper Nodehandle */ +#include <px4.h> #include <platforms/px4_nodehandle.h> namespace px4 { bool task_should_exit = false; + + +void NodeHandle::spinOnce() { + /* Loop through subscriptions, call callback for updated subscriptions */ + uORB::SubscriptionNode *sub = _subs.getHead(); + int count = 0; + + while (sub != nullptr) { + if (count++ > kMaxSubscriptions) { + PX4_WARN("exceeded max subscriptions"); + break; + } + + sub->update(); + sub = sub->getSibling(); + } +} + } diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 6471e3e38..22daee823 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -59,16 +59,4 @@ bool ok() return !task_should_exit; } -void spin_once() -{ - // XXX check linked list of topics with orb_check() here - -} - -void spin() -{ - // XXX block waiting for updated topics here - -} - } diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index b665f3fe7..f473bf629 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -105,10 +105,12 @@ public: ~NodeHandle() {}; template<typename M> - Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) { + Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) { + // Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(int i)> callback) { + // Subscriber * subscribe(const struct orb_metadata *meta, CallbackFunction callback) { unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? //XXX - Subscriber *sub = new Subscriber(meta, interval, fp, &_subs); + Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs); return sub; } @@ -118,7 +120,14 @@ public: Publisher * pub = new Publisher(meta, &_pubs); return pub; } + + void spinOnce(); + + void spin() { + //XXX: call callbacks and do not return until task is terminated + } private: + static const uint16_t kMaxSubscriptions = 100; List<uORB::SubscriptionNode*> _subs; List<uORB::PublicationNode*> _pubs; diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 12d422bb3..71c3a766d 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -44,6 +44,7 @@ /* includes when building for NuttX */ #include <uORB/Subscription.hpp> #include <containers/List.hpp> +#include <functional> #endif namespace px4 @@ -61,23 +62,46 @@ private: ros::Subscriber _ros_sub; }; #else -class Subscriber : - public uORB::SubscriptionNode +// typedef std::function<void(int)> CallbackFunction; +class Subscriber +{ +public: + Subscriber() {}; + ~Subscriber() {}; +private: +}; + +template<typename M> +class SubscriberPX4 : + public Subscriber, + public uORB::Subscription<M> { public: - template<typename M> - Subscriber(const struct orb_metadata *meta, + SubscriberPX4(const struct orb_metadata *meta, unsigned interval, - void(*fp)(M), + std::function<void(const M&)> callback, + // std::function<void(int i)> callback, + // CallbackFunction callback, List<uORB::SubscriptionNode *> * list) : - uORB::SubscriptionNode(meta, interval, list) + Subscriber(), + uORB::Subscription<M>(meta, interval, list) //XXX store callback {} - ~Subscriber() {}; + ~SubscriberPX4() {}; void update() { - //XXX list traversal callback, needed? - } ; + /* get latest data */ + uORB::Subscription<M>::update(); + + /* Call callback which performs actions based on this data */ + // _callback(); + + }; +private: + // std::function<void(int i)> _callback; + // CallbackFunction _callback; + std::function<void(const M&)> _callback; + }; #endif |