From c2e2b3d52fa7bcfbbec110b5555d2c437657f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 17:04:15 +0100 Subject: make param wrapper macros compatible for px4 and ros, needs cleanup --- src/examples/subscriber/subscriber.cpp | 5 ++-- src/examples/subscriber/subscriber_params.c | 7 +++-- src/examples/subscriber/subscriber_params.h | 43 +++++++++++++++++++++++++++++ src/platforms/px4_defines.h | 21 ++++++++++---- 4 files changed, 66 insertions(+), 10 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.h (limited to 'src') diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index cc9b7a794..68003b6df 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,6 +26,7 @@ */ #include +#include "subscriber_params.h" using namespace px4; @@ -75,8 +76,8 @@ PX4_MAIN_FUNCTION(subscriber) { px4::NodeHandle n; /* 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); + px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); /* Read the parameter back for testing */ int32_t sub_interval; diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index e28bfbc36..0d36f5021 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -38,18 +38,19 @@ * @author Thomas Gubler */ -#include +#include +#include "subscriber_params.h" /** * Interval of one subscriber in the example in ms * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, 100); +PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); +PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 000000000..3f3ff5bce --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file subscriber_params.h + * Default parameters for the subscriber example + * + * @author Thomas Gubler + */ +#pragma once + +#define SUB_INTERV_DEFAULT_VALUE 100 +#define SUB_TESTF_DEFAULT_VALUE 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a71507510..6760f57ea 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,11 +39,15 @@ #pragma once +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE + + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT +#include "ros/ros.h" #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO @@ -51,18 +55,21 @@ #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); -typedef const std::string px4_param_t; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { +typedef const char* px4_param_t; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int 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) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *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 PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +#define PX4_PARAM_INT32_T int //XXX #else /* @@ -78,8 +85,9 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(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_INIT(_name) param_find(#_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) +#define PX4_PARAM_INT32_T int32_t #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ @@ -92,3 +100,6 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; +// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -- cgit v1.2.3