improve param wrapper for ros, prepare for px4

4 files changed, 25 insertions, 6 deletions

diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index a8ecd4a7d..e52d661a9 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -74,13 +74,17 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 */
 	px4::NodeHandle n;
 
-	/* Define a parameter */
-	PX4_PARAM_INIT("SUB_INTERV", 100);
+	/* Define parameters */
+	px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100);
+	px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f);
 
 	/* Read the parameter back for testing */
 	int32_t sub_interval;
-	PX4_PARAM_GET("SUB_INTERV", &sub_interval);
+	float test_float;
+	PX4_PARAM_GET(p_sub_interv, &sub_interval);
 	PX4_INFO("Param SUB_INTERV = %d", sub_interval);
+	PX4_PARAM_GET(p_test_float, &test_float);
+	PX4_INFO("Param SUB_TESTF = %f", (double)test_float);
 
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
@@ -97,7 +101,7 @@ PX4_MAIN_FUNCTION(subscriber) {
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
+	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
 	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
 	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
 	PX4_INFO("subscribed");
diff --git a/src/include/px4.h b/src/include/px4.h
index 22d661b17..2d5d25964 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -56,6 +56,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
 #include <systemlib/err.h>
+#include <systemlib/param/param.h>
 
 #endif
 
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 2d9051aae..a10df858a 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -51,8 +51,18 @@
 #define PX4_TOPIC_T(_name) _name
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
-#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default);
-#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt)
+typedef const std::string px4_param_t;
+static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) {
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) {
+	ros::param::set(name, value);
+	return (px4_param_t)name;
+};
+#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default)
+// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default)
+#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 #else
 /*
  * Building for NuttX
@@ -65,6 +75,9 @@
 #define PX4_TOPIC_T(_name) _name##_s
 #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+typedef param_t px4_param_t
+#define PX4_PARAM_INIT(_name, _default) param_find(_name)
+#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
 #endif
 
 /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index cf47fec59..a2775d69a 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -99,6 +99,7 @@ public:
 	void spin() { ros::spin(); }
 
 	void spinOnce() { ros::spinOnce(); }
+
 private:
 	static const uint32_t kQueueSizeDefault = 1000;
 	std::list<Subscriber*> _subs;