diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-28 14:00:02 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-28 14:00:02 +0100 |
commit | 1b416a8e1f2b54e183d61a0022bb48de47576a6c (patch) | |
tree | 57116a1dcb906d3a1226dd28200c1bcad57d7017 /src/platforms | |
parent | b0cfc2d1226c2b62a6bf3aac1809458b04902728 (diff) | |
download | px4-firmware-1b416a8e1f2b54e183d61a0022bb48de47576a6c.tar.gz px4-firmware-1b416a8e1f2b54e183d61a0022bb48de47576a6c.tar.bz2 px4-firmware-1b416a8e1f2b54e183d61a0022bb48de47576a6c.zip |
use interval setting correctly, improve px4::spin
Diffstat (limited to 'src/platforms')
-rw-r--r-- | src/platforms/px4_defines.h | 4 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 40 |
2 files changed, 34 insertions, 10 deletions
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 79f305140..7efe13bae 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -50,7 +50,7 @@ #define PX4_INFO ROS_INFO #define PX4_TOPIC(_name) #_name #define PX4_TOPIC_T(_name) _name -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); #else /* @@ -65,6 +65,6 @@ #define PX4_INFO warnx #define PX4_TOPIC(_name) ORB_ID(_name) #define PX4_TOPIC_T(_name) _name##_s -#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}) +#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval) #endif diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index e228dfca5..ced844fd5 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -49,6 +49,7 @@ #include <inttypes.h> #else /* includes when building for NuttX */ +#include <poll.h> #endif namespace px4 @@ -99,17 +100,23 @@ class __EXPORT NodeHandle public: NodeHandle() : _subs(), - _pubs() + _pubs(), + _sub_min_interval(nullptr) {} ~NodeHandle() {}; template<typename M> - Subscriber * subscribe(const struct orb_metadata *meta, std::function<void(const M&)> callback) { - unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? - //XXX - Subscriber *sub = new SubscriberPX4<M>(meta, interval, callback, &_subs); - return sub; + Subscriber * subscribe(const struct orb_metadata *meta, + std::function<void(const M&)> callback, + unsigned interval) { + SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs); + + /* Check if this is the smallest interval so far and update _sub_min_interval */ + if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() < sub_px4->getInterval()) { + _sub_min_interval = sub_px4; + } + return (Subscriber*)sub_px4; } template<typename M> @@ -122,7 +129,23 @@ public: void spinOnce(); void spin() { - while (true) { //XXX + while (true) { //XXX check for termination + const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ + if (_sub_min_interval == nullptr) { + usleep(timeout_ms * 1000); + continue; + } + + /* Poll fd with smallest interval */ + struct pollfd pfd; + pfd.fd = _sub_min_interval->getHandle(); + pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { + /* timed out */ + continue; + } + spinOnce(); } } @@ -130,7 +153,8 @@ private: static const uint16_t kMaxSubscriptions = 100; List<uORB::SubscriptionNode*> _subs; List<uORB::PublicationNode*> _pubs; - + uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + of all Subscriptions in _subs*/ }; #endif } |