dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add update_sub_min_interval function

This commit is contained in:
Thomas Gubler 2015-01-23 08:27:06 +01:00
parent 57569482ad
commit af943cf16a
1 changed files with 16 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/platforms/px4_nodehandle.h
View File

@ -132,7 +132,7 @@ public:
void spin() { ros::spin(); }
private:
protected:
std::list<SubscriberBase *> _subs; /**< Subcriptions of node */
std::list<PublisherBase *> _pubs; /**< Publications of node */
};
@ -189,13 +189,8 @@ public:
Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval
{
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub_px4;
}
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
@ -208,13 +203,8 @@ public:
Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10)
{
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub_px4;
}
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
@ -227,13 +217,8 @@ public:
Subscriber<T> *subscribe(unsigned interval=10) //XXX interval
{
SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
/* Check if this is the smallest interval so far and update _sub_min_interval */
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub_px4;
}
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
@ -290,13 +275,24 @@ public:
spinOnce();
}
}
private:
protected:
static const uint16_t kMaxSubscriptions = 100;
static const uint16_t kMaxPublications = 100;
List<SubscriberNode *> _subs; /**< Subcriptions of node */
List<PublisherNode *> _pubs; /**< Publications of node */
SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
of all Subscriptions in _subs*/
/**
* Check if this is the smallest interval so far and update _sub_min_interval
*/
template<typename T>
void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub)
{
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub;
}
}
};
#endif
}