forked from Archive/PX4-Autopilot
WIP, c++11 style callbacks for px4
This commit is contained in:
parent
486d81cb95
commit
36bf0c04c8
|
@ -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 \
|
||||
|
|
|
@ -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");
|
||||
|
||||
/**
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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:
|
||||
template<typename M>
|
||||
Subscriber(const struct orb_metadata *meta,
|
||||
Subscriber() {};
|
||||
~Subscriber() {};
|
||||
private:
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
class SubscriberPX4 :
|
||||
public Subscriber,
|
||||
public uORB::Subscription<M>
|
||||
{
|
||||
public:
|
||||
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
|
||||
|
||||
|
|
Loading…
Reference in New Issue