From e7c1e5b1ff7b1bbdc11ab2cae6b99fe459487119 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 11:36:23 +0100 Subject: wip, working on the nuttx wrapper --- src/examples/publisher/publisher.cpp | 13 +++++++------ src/examples/subscriber/subscriber.cpp | 11 ++++++----- src/include/px4.h | 10 ++++------ src/modules/uORB/Publication.cpp | 1 + src/modules/uORB/Publication.hpp | 10 +++++----- src/modules/uORB/Subscription.cpp | 1 + src/platforms/nuttx/module.mk | 3 ++- src/platforms/nuttx/px4_nodehandle.cpp | 2 +- src/platforms/nuttx/px4_nuttx_impl.cpp | 8 +++++--- src/platforms/nuttx/px4_publisher.cpp | 1 + src/platforms/nuttx/px4_subscriber.cpp | 2 +- src/platforms/px4_defines.h | 23 ++++++++++++++++++++--- src/platforms/px4_middleware.h | 11 ++++++----- src/platforms/px4_nodehandle.h | 23 ++++++++++++----------- src/platforms/px4_publisher.h | 21 +++++++++++++++------ src/platforms/px4_subscriber.h | 20 ++++++++++++++------ 16 files changed, 101 insertions(+), 59 deletions(-) (limited to 'src') diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index c09cca1fc..68fd8a5c3 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -26,14 +26,15 @@ */ #include -#include -#include +using namespace px4; /** * This tutorial demonstrates simple sending of messages over the PX4 middleware system. */ -int main(int argc, char **argv) +// __EXPORT bool task_should_exit; + +PX4_MAIN_FUNCTION(publisher) { px4::init(argc, argv, "px4_publisher"); @@ -57,7 +58,7 @@ int main(int argc, char **argv) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - px4::Publisher rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); + px4::Publisher * rc_channels_pub = n.advertise(PX4_TOPIC(rc_channels)); px4::Rate loop_rate(10); @@ -72,7 +73,7 @@ int main(int argc, char **argv) /** * This is a message object. You stuff it with data, and then publish it. */ - px4::rc_channels msg; + PX4_TOPIC_T(rc_channels) msg; msg.timestamp_last_valid = px4::get_time_micros(); PX4_INFO("%lu", msg.timestamp_last_valid); @@ -83,7 +84,7 @@ int main(int argc, char **argv) * given as a template parameter to the advertise<>() call, as was done * in the constructor above. */ - rc_channels_pub.publish(msg); + rc_channels_pub->publish(msg); px4::spin_once(); diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index b91859027..cdadaf2bc 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,19 +26,20 @@ */ #include -#include "px4/rc_channels.h" + +using namespace px4; /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -void rc_channels_callback(const px4::rc_channels::ConstPtr &msg) +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) *msg) { PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid); } -PX4_MAIN_FUNCTION(subscriber) +// __EXPORT bool task_should_exit; -int main(int argc, char **argv) +PX4_MAIN_FUNCTION(subscriber) { /** * The ros::init() function needs to see argc and argv so that it can perform @@ -74,7 +75,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); + n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/px4.h b/src/include/px4.h index 391972b12..22d661b17 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -46,19 +46,17 @@ * Building for running within the ROS environment */ #include "ros/ros.h" +#include "px4/rc_channels.h" -#define PX4_WARN ROS_WARN -#define PX4_INFO ROS_INFO -#define PX4_TOPIC(name) #name #else /* * Building for NuttX */ +#include #include +#include +#include -#define PX4_WARN warnx -#define PX4_INFO warnx -#define PX4_TOPIC(name) ORB_ID(name) #endif #include "../platforms/px4_defines.h" diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 12ef83aa0..05605417d 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -48,6 +48,7 @@ #include "topics/actuator_outputs.h" #include "topics/encoders.h" #include "topics/tecs_status.h" +#include "topics/rc_channels.h" namespace uORB { diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 1c48929e4..fd1ee4dec 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -59,7 +59,7 @@ public: * Constructor * * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. */ PublicationBase(const struct orb_metadata *meta) : @@ -96,7 +96,7 @@ protected: orb_advert_t _handle; }; -/** +/** * alias class name so it is clear that the base class * can be used by itself if desired */ @@ -114,9 +114,9 @@ public: * Constructor * * - * @param meta The uORB metadata (usually from the ORB_ID() + * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * @param list A pointer to a list of subscriptions + * @param list A pointer to a list of subscriptions * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, @@ -144,7 +144,7 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from + * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. * @param list A list interface for adding to * list during construction diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index a681ccb30..61609d009 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -52,6 +52,7 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" +#include "topics/rc_channels.h" namespace uORB { diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk index 128f0e734..1c0ad7aa4 100644 --- a/src/platforms/nuttx/module.mk +++ b/src/platforms/nuttx/module.mk @@ -37,6 +37,7 @@ SRCS = px4_nuttx_impl.cpp \ px4_publisher.cpp \ - px4_subscriber.cpp + px4_subscriber.cpp \ + px4_nodehandle.cpp MAXOPTIMIZATION = -Os diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp index 9d43daa49..473a5cf77 100644 --- a/src/platforms/nuttx/px4_nodehandle.cpp +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -36,4 +36,4 @@ * * PX4 Middleware Wrapper Nodehandle */ -#include +#include diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 3a6529716..4b87f68fe 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -38,6 +38,7 @@ */ #include +#include extern bool task_should_exit; @@ -46,8 +47,7 @@ namespace px4 void init(int argc, char *argv[], const char *process_name) { - px4_warn("process: %s", process_name); - return 0; + PX4_WARN("process: %s", process_name); } uint64_t get_time_micros() @@ -57,7 +57,9 @@ uint64_t get_time_micros() bool ok() { - return !task_should_exit; + // return !task_should_exit; + //XXX + return true; } void spin_once() diff --git a/src/platforms/nuttx/px4_publisher.cpp b/src/platforms/nuttx/px4_publisher.cpp index ab6035b22..3bd70272f 100644 --- a/src/platforms/nuttx/px4_publisher.cpp +++ b/src/platforms/nuttx/px4_publisher.cpp @@ -36,5 +36,6 @@ * * PX4 Middleware Wrapper for Publisher */ +#include diff --git a/src/platforms/nuttx/px4_subscriber.cpp b/src/platforms/nuttx/px4_subscriber.cpp index 088c08fdb..426e646c9 100644 --- a/src/platforms/nuttx/px4_subscriber.cpp +++ b/src/platforms/nuttx/px4_subscriber.cpp @@ -36,5 +36,5 @@ * * PX4 Middleware Wrapper Subscriber */ - +#include diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 48234766f..d4dc71453 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -44,8 +44,25 @@ * Building for running within the ROS environment */ #define __EXPORT -#define PX4_MAIN_FUNCTION(_prefix) +// #define PX4_MAIN_FUNCTION(_prefix) +#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) +#define PX4_WARN ROS_WARN +#define PX4_INFO ROS_INFO +#define PX4_TOPIC(name) #name +#define PX4_TOPIC_T(name) name + #else -#include -#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); } +/* + * Building for NuttX + */ + +// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); } +// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); } +#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) +#define PX4_WARN warnx +#define PX4_WARN warnx +#define PX4_INFO warnx +#define PX4_TOPIC(name) ORB_ID(name) +#define PX4_TOPIC_T(name) name##_s + #endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index d1c0656af..eebfc9049 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -40,19 +40,20 @@ #pragma once #include +#include namespace px4 { -void init(int argc, char *argv[], const char *process_name); +__EXPORT void init(int argc, char *argv[], const char *process_name); -uint64_t get_time_micros(); +__EXPORT uint64_t get_time_micros(); -bool ok(); +__EXPORT bool ok(); -void spin_once(); +__EXPORT void spin_once(); -void spin(); +__EXPORT void spin(); class Rate { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index fa2c8d6a4..34a605647 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -39,8 +39,8 @@ #pragma once /* includes for all platforms */ -#include -#include +#include "px4_subscriber.h" +#include "px4_publisher.h" #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -49,7 +49,6 @@ #include #else /* includes when building for NuttX */ -#include #endif namespace px4 @@ -90,7 +89,7 @@ private: std::list _pubs; }; #else -class NodeHandle +class __EXPORT NodeHandle { public: NodeHandle() : @@ -101,20 +100,22 @@ public: ~NodeHandle() {}; template - Subscriber subscribe(const char *topic, void(*fp)(M)) { - Subscriber sub(&_subs, , interval); + Subscriber * subscribe(const struct orb_metadata *meta, void(*fp)(M)) { + unsigned interval = 0;//XXX decide how to wrap this, ros equivalent? + //XXX + Subscriber *sub = new Subscriber(meta, interval, fp, &_subs); return sub; } template - Publisher advertise(const char *topic) { - Publisher pub(ros_pub); - _pubs.push_back(pub); + Publisher * advertise(const struct orb_metadata *meta) { + //XXX + Publisher * pub = new Publisher(meta, &_pubs); return pub; } private: - List _subs; - List _pubs; + List _subs; + List _pubs; }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 53e63b695..9ce211d25 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -43,6 +43,7 @@ #else /* includes when building for NuttX */ #include +#include #endif namespace px4 @@ -60,16 +61,24 @@ private: ros::Publisher _ros_pub; }; #else -template class Publisher : - public uORB::Publication + public uORB::PublicationNode +{ public: - Publisher(List * list, - const struct orb_metadata *meta, unsigned interval) : - uORB::Publication(list, meta) + Publisher(const struct orb_metadata *meta, + List * list) : + uORB::PublicationNode(meta, list) {} ~Publisher() {}; -{ + template + int publish(const M &msg) { + uORB::PublicationBase::update((void*)&msg); + return 0; + } + + void update() { + //XXX list traversal callback, needed? + } ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6312e0cbe..12d422bb3 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -43,6 +43,7 @@ #else /* includes when building for NuttX */ #include +#include #endif namespace px4 @@ -60,16 +61,23 @@ private: ros::Subscriber _ros_sub; }; #else -template class Subscriber : - public uORB::Subscription + public uORB::SubscriptionNode +{ public: - Subscriber(List * list, - const struct orb_metadata *meta, unsigned interval) : - uORB::Subsciption(list, meta, interval) + template + Subscriber(const struct orb_metadata *meta, + unsigned interval, + void(*fp)(M), + List * list) : + uORB::SubscriptionNode(meta, interval, list) + //XXX store callback {} ~Subscriber() {}; -{ + + void update() { + //XXX list traversal callback, needed? + } ; }; #endif -- cgit v1.2.3