wip, working on the nuttx wrapper

16 files changed, 101 insertions, 59 deletions

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 <px4.h>
-#include <px4/rc_channels.h>
 
-#include <sstream>
+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::rc_channels>(PX4_TOPIC(rc_channels));
+	px4::Publisher * rc_channels_pub = n.advertise<PX4_TOPIC_T(rc_channels)>(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 <px4.h>
-#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 <nuttx/config.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <systemlib/err.h>
 
-#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 <px4_nodehandle.h>
+#include <platforms/px4_nodehandle.h>
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 <px4.h>
+#include <drivers/drv_hrt.h>
 
 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 <platforms/px4_publisher.h>
 
 
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 <platforms/px4_subscriber.h>
 
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 <nuttx/config.h>
-#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 <stdint.h>
+#include <unistd.h>
 
 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 <px4_subscriber.h>
-#include <px4_publisher.h>
+#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 <inttypes.h>
 #else
 /* includes when building for NuttX */
-#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -90,7 +89,7 @@ private:
 	std::list<Publisher> _pubs;
 };
 #else
-class NodeHandle
+class __EXPORT NodeHandle
 {
 public:
 	NodeHandle() :
@@ -101,20 +100,22 @@ public:
 	~NodeHandle() {};
 
 	template<typename M>
-	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<typename M>
-	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<Subscriber> _subs;
-	List<Publisher> _pubs;
+	List<uORB::SubscriptionNode*> _subs;
+	List<uORB::PublicationNode*> _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 <uORB/Publication.hpp>
+#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -60,16 +61,24 @@ private:
 	ros::Publisher _ros_pub;
 };
 #else
-template<typename M>
 class Publisher :
-	public uORB::Publication<M>
+	public uORB::PublicationNode
+{
 public:
-	Publisher(List<SubscriptionBase *> * list,
-			const struct orb_metadata *meta, unsigned interval) :
-		uORB::Publication(list, meta)
+	Publisher(const struct orb_metadata *meta,
+			List<uORB::PublicationNode *> * list) :
+		uORB::PublicationNode(meta, list)
 	{}
 	~Publisher() {};
-{
+	template<typename M>
+	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 <uORB/Subscription.hpp>
+#include <containers/List.hpp>
 #endif
 
 namespace px4
@@ -60,16 +61,23 @@ private:
 	ros::Subscriber _ros_sub;
 };
 #else
-template<typename M>
 class Subscriber :
-	public uORB::Subscription<M>
+	public uORB::SubscriptionNode
+{
 public:
-	Subscriber(List<SubscriptionBase *> * list,
-			const struct orb_metadata *meta, unsigned interval) :
-		uORB::Subsciption(list, meta, interval)
+	template<typename M>
+	Subscriber(const struct orb_metadata *meta,
+			unsigned interval,
+			void(*fp)(M),
+			List<uORB::SubscriptionNode *> * list) :
+		uORB::SubscriptionNode(meta, interval, list)
+		//XXX store callback
 	{}
 	~Subscriber() {};
-{
+
+	void update() {
+	//XXX list traversal callback, needed?
+	} ;
 };
 #endif