wip, working on the nuttx wrapper

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

src/platforms/nuttx/px4_nodehandle.cpp

@@ -36,4 +36,4 @@
  *
  * PX4 Middleware Wrapper Nodehandle
  */
-#include <px4_nodehandle.h>
+#include <platforms/px4_nodehandle.h>

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()

src/platforms/nuttx/px4_publisher.cpp

@@ -36,5 +36,6 @@
  *
  * PX4 Middleware Wrapper for Publisher
  */
+#include <platforms/px4_publisher.h>
 
 

src/platforms/nuttx/px4_subscriber.cpp

@@ -36,5 +36,5 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
-
+#include <platforms/px4_subscriber.h>
 

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

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
 {

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

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
 }

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