From 4d74daf4a02e4a440c20d864e42355b43f55e0ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 7 Feb 2015 10:11:47 +0100 Subject: [PATCH 01/12] ros launch files: package has a new name --- launch/gazebo_ardrone_empty_world.launch | 2 +- launch/gazebo_ardrone_house_world.launch | 2 +- launch/gazebo_iris_empty_world.launch | 2 +- launch/gazebo_iris_house_world.launch | 2 +- launch/gazebo_iris_outdoor_world.launch | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 395e70b006..d04991f19c 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index f43c16b509..d1bb2043aa 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 833aaf07aa..d69b7d0069 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch index bfdf9fe311..2e88e157c9 100644 --- a/launch/gazebo_iris_house_world.launch +++ b/launch/gazebo_iris_house_world.launch @@ -1,6 +1,6 @@ - + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index c2975a050d..a1df06496b 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -1,6 +1,6 @@ - + From df5ad88df7d925278356f9480a0c2418c0316c9d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 12:34:03 +0100 Subject: [PATCH 02/12] add px4 parameter api class (with ros implementation) --- src/include/px4.h | 1 + src/platforms/px4_parameter.h | 123 ++++++++++++++++++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 src/platforms/px4_parameter.h diff --git a/src/include/px4.h b/src/include/px4.h index 34137ee6f4..b93ab5f80b 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -45,4 +45,5 @@ #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" #include "../platforms/px4_subscriber.h" +#include "../platforms/px4_parameter.h" #endif diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h new file mode 100644 index 0000000000..464a84f517 --- /dev/null +++ b/src/platforms/px4_parameter.h @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 px4_parameter.h + * + * PX4 Parameter API + */ +#pragma once +#if defined(__PX4_ROS) +/* includes when building for ros */ +#include "ros/ros.h" +#include +#else +/* includes when building for NuttX */ +#endif + +namespace px4 +{ + +/** + * Parameter base class + */ +template +class __EXPORT ParameterBase +{ +public: + ParameterBase(const char *name, T value) : + _name(name), + _value(value) + {} + + virtual ~ParameterBase() {}; + + /** + * Update the parameter value and return the value + */ + virtual T update() = 0; + + /** + * Get the current parameter value + */ + virtual T get() = 0; + +protected: + const char *_name; /** The parameter name */ + T _value; /**< The current value of the parameter */ + +}; + +#if defined(__PX4_ROS) +template +class Parameter : + public ParameterBase +{ +public: + Parameter(const char *name, T value) : + ParameterBase(name, value) + { + if (!ros::param::has(name)) { + ros::param::set(name, value); + } + } + + ~Parameter() {}; + + /** + * Update the parameter value + * Nothing to do for ROS + */ + T update() + { + ros::param::get(this->_name, this->_value); + return get(); + } + + /** + * Get the current parameter value + */ + T get() + { + return this->_value; + } + +}; + + +#else +#endif + +typedef Parameter ParameterFloat; +typedef Parameter ParameterInt; +} From 69f5726d7027ef45ccbb1a568ff33e91bd742654 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 12:35:10 +0100 Subject: [PATCH 03/12] make subscriber example use the new param api --- .../subscriber/subscriber_example.cpp | 24 +++++++++---------- src/examples/subscriber/subscriber_example.h | 7 +++--- 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index e1292f7888..496e1dcd8f 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -49,20 +49,18 @@ void rc_channels_callback_function(const px4_rc_channels &msg) { SubscriberExample::SubscriberExample() : _n(), - _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)), - _interval(0), - _p_test_float(PX4_PARAM_INIT(SUB_TESTF)), - _test_float(0.0f) + _p_sub_interv("SUB_INTERV", PARAM_SUB_INTERV_DEFAULT), + _p_test_float("SUB_TESTF", PARAM_SUB_TESTF_DEFAULT) { /* Read the parameter back as example */ - PX4_PARAM_GET(_p_sub_interv, &_interval); - PX4_INFO("Param SUB_INTERV = %d", _interval); - PX4_PARAM_GET(_p_test_float, &_test_float); - PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); + _p_sub_interv.update(); + _p_test_float.update(); + PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_p_test_float.get()); /* Do some subscriptions */ /* Function */ - _n.subscribe(rc_channels_callback_function, _interval); + _n.subscribe(rc_channels_callback_function, _p_sub_interv.get()); /* No callback */ _sub_rc_chan = _n.subscribe(500); @@ -98,8 +96,8 @@ void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &ms void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { PX4_INFO("parameter_update_callback (method): [%llu]", msg.data().timestamp); - PX4_PARAM_GET(_p_sub_interv, &_interval); - PX4_INFO("Param SUB_INTERV = %d", _interval); - PX4_PARAM_GET(_p_test_float, &_test_float); - PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float); + _p_sub_interv.update(); + PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); + _p_test_float.update(); + PX4_INFO("Param SUB_TESTF = %.3f", (double)_p_test_float.get()); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 9b6d890e3b..686fed023e 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -52,12 +52,11 @@ public: void spin() {_n.spin();} protected: px4::NodeHandle _n; - px4_param_t _p_sub_interv; - int32_t _interval; - px4_param_t _p_test_float; - float _test_float; + px4::ParameterInt _p_sub_interv; + px4::ParameterFloat _p_test_float; px4::Subscriber * _sub_rc_chan; + void rc_channels_callback(const px4_rc_channels &msg); void vehicle_attitude_callback(const px4_vehicle_attitude &msg); void parameter_update_callback(const px4_parameter_update &msg); From bc8efeb73eaa77c1835823d96ca5d8ca2a76b24d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 14:10:57 +0100 Subject: [PATCH 04/12] add param api also for firmware builds --- src/platforms/px4_parameter.h | 42 +++++++++++++++++++++++++++++++---- 1 file changed, 38 insertions(+), 4 deletions(-) diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h index 464a84f517..6b5453d773 100644 --- a/src/platforms/px4_parameter.h +++ b/src/platforms/px4_parameter.h @@ -43,6 +43,7 @@ #include #else /* includes when building for NuttX */ +#include #endif namespace px4 @@ -96,7 +97,6 @@ public: /** * Update the parameter value - * Nothing to do for ROS */ T update() { @@ -113,11 +113,45 @@ public: } }; - - #else +template +class Parameter : + public ParameterBase +{ +public: + Parameter(const char *name, T value) : + ParameterBase(name, value), + _handle(param_find(name)) + { + } + + ~Parameter() {}; + + /** + * Update the parameter value + */ + T update() + { + if (_handle != PARAM_INVALID) { + param_get(_handle, &(this->_value)); + } + return get(); + } + + /** + * Get the current parameter value + */ + T get() + { + return this->_value; + } + +protected: + param_t _handle; + +}; #endif -typedef Parameter ParameterFloat; +typedef Parameter ParameterFloat; typedef Parameter ParameterInt; } From 1097f5f55c8ba535de1e4245e8dd1ad1096924a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 14:11:35 +0100 Subject: [PATCH 05/12] fix comments --- src/platforms/px4_publisher.h | 4 ++-- src/platforms/px4_subscriber.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index d9cd7a3c19..eb8e964e79 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file px4_nodehandle.h + * @file px4_publisher.h * - * PX4 Middleware Wrapper Node Handle + * PX4 Publisher API, implements publishing of messages from a nodehandle */ #pragma once #if defined(__PX4_ROS) diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6ca35b173f..9b0ca1319a 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -34,7 +34,7 @@ /** * @file px4_subscriber.h * - * PX4 Middleware Wrapper Subscriber + * PX4 Subscriber API, implements subscribing to messages from a nodehandle */ #pragma once From 04cc8922e40b4fa21a525f73defdd21cfd0e9546 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 15:50:02 +0100 Subject: [PATCH 06/12] param api: load param value correctly --- src/platforms/px4_parameter.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h index 6b5453d773..2f3c6323ee 100644 --- a/src/platforms/px4_parameter.h +++ b/src/platforms/px4_parameter.h @@ -91,6 +91,8 @@ public: if (!ros::param::has(name)) { ros::param::set(name, value); } + + update(); } ~Parameter() {}; @@ -123,6 +125,7 @@ public: ParameterBase(name, value), _handle(param_find(name)) { + update(); } ~Parameter() {}; From 18c7aa9993f24c05b04d816cdd8e0195fccc3b88 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 15:51:01 +0100 Subject: [PATCH 07/12] param api: run fix code style script --- src/platforms/px4_parameter.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h index 2f3c6323ee..1f37fc700a 100644 --- a/src/platforms/px4_parameter.h +++ b/src/platforms/px4_parameter.h @@ -138,6 +138,7 @@ public: if (_handle != PARAM_INVALID) { param_get(_handle, &(this->_value)); } + return get(); } From fbcf2c8fb50af65f03192b759ef31b73297d6114 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 16:17:15 +0100 Subject: [PATCH 08/12] add missing __EXPORT --- src/platforms/px4_parameter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h index 1f37fc700a..1376320b1c 100644 --- a/src/platforms/px4_parameter.h +++ b/src/platforms/px4_parameter.h @@ -117,7 +117,7 @@ public: }; #else template -class Parameter : +class __EXPORT Parameter : public ParameterBase { public: From 893beb64a023d736d838a79b2f6ee54951dc2a65 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 16:31:59 +0100 Subject: [PATCH 09/12] mc att ctrl multiplatform use new param api --- .../mc_att_control.cpp | 107 +++++++----------- .../mc_att_control.h | 41 ++++--- 2 files changed, 64 insertions(+), 84 deletions(-) diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 1e40c2b056..ecbf98c9ec 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp @@ -73,31 +73,34 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(nullptr), _n(), + /* parameters */ + _params_handles({ + .roll_p = px4::ParameterFloat("MC_ROLL_P", PARAM_MC_ROLL_P_DEFAULT), + .roll_rate_p = px4::ParameterFloat("MC_ROLLRATE_P", PARAM_MC_ROLLRATE_P_DEFAULT), + .roll_rate_i = px4::ParameterFloat("MC_ROLLRATE_I", PARAM_MC_ROLLRATE_I_DEFAULT), + .roll_rate_d = px4::ParameterFloat("MC_ROLLRATE_D", PARAM_MC_ROLLRATE_D_DEFAULT), + .pitch_p = px4::ParameterFloat("MC_PITCH_P", PARAM_MC_PITCH_P_DEFAULT), + .pitch_rate_p = px4::ParameterFloat("MC_PITCHRATE_P", PARAM_MC_PITCHRATE_P_DEFAULT), + .pitch_rate_i = px4::ParameterFloat("MC_PITCHRATE_I", PARAM_MC_PITCHRATE_I_DEFAULT), + .pitch_rate_d = px4::ParameterFloat("MC_PITCHRATE_D", PARAM_MC_PITCHRATE_D_DEFAULT), + .yaw_p = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT), + .yaw_rate_p = px4::ParameterFloat("MC_YAWRATE_P", PARAM_MC_YAWRATE_P_DEFAULT), + .yaw_rate_i = px4::ParameterFloat("MC_YAWRATE_I", PARAM_MC_YAWRATE_I_DEFAULT), + .yaw_rate_d = px4::ParameterFloat("MC_YAWRATE_D", PARAM_MC_YAWRATE_D_DEFAULT), + .yaw_ff = px4::ParameterFloat("MC_YAW_FF", PARAM_MC_YAW_FF_DEFAULT), + .yaw_rate_max = px4::ParameterFloat("MC_YAWRATE_MAX", PARAM_MC_YAWRATE_MAX_DEFAULT), + .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MC_MAN_Y_MAX_DEFAULT), + .acro_roll_max = px4::ParameterFloat("MC_ACRO_R_MAX", PARAM_MC_ACRO_R_MAX_DEFAULT), + .acro_pitch_max = px4::ParameterFloat("MC_ACRO_P_MAX", PARAM_MC_ACRO_P_MAX_DEFAULT), + .acro_yaw_max = px4::ParameterFloat("MC_ACRO_Y_MAX", PARAM_MC_ACRO_Y_MAX_DEFAULT) + }), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P); - _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P); - _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I); - _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D); - _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P); - _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P); - _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I); - _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D); - _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P); - _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P); - _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I); - _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D); - _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF); - _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX); - _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX); - _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX); - _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX); - _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX); - _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX); - _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX); - /* fetch initial parameter values */ parameters_update(); @@ -113,7 +116,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _manual_control_sp = _n.subscribe(0); _armed = _n.subscribe(0); _v_status = _n.subscribe(0); - } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -123,57 +125,36 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() int MulticopterAttitudeControl::parameters_update() { - float v; - /* roll gains */ - PX4_PARAM_GET(_params_handles.roll_p, &v); - _params.att_p(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_i, &v); - _params.rate_i(0) = v; - PX4_PARAM_GET(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; + _params.att_p(0) = _params_handles.roll_p.update(); + _params.rate_p(0) = _params_handles.roll_rate_p.update(); + _params.rate_i(0) = _params_handles.roll_rate_i.update(); + _params.rate_d(0) = _params_handles.roll_rate_d.update(); /* pitch gains */ - PX4_PARAM_GET(_params_handles.pitch_p, &v); - _params.att_p(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_i, &v); - _params.rate_i(1) = v; - PX4_PARAM_GET(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; + _params.att_p(1) = _params_handles.pitch_p.update(); + _params.rate_p(1) = _params_handles.pitch_rate_p.update(); + _params.rate_i(1) = _params_handles.pitch_rate_i.update(); + _params.rate_d(1) = _params_handles.pitch_rate_d.update(); /* yaw gains */ - PX4_PARAM_GET(_params_handles.yaw_p, &v); - _params.att_p(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_p, &v); - _params.rate_p(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_i, &v); - _params.rate_i(2) = v; - PX4_PARAM_GET(_params_handles.yaw_rate_d, &v); - _params.rate_d(2) = v; + _params.att_p(2) = _params_handles.yaw_p.update(); + _params.rate_p(2) = _params_handles.yaw_rate_p.update(); + _params.rate_i(2) = _params_handles.yaw_rate_i.update(); + _params.rate_d(2) = _params_handles.yaw_rate_d.update(); - PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff); - PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max); - _params.yaw_rate_max = math::radians(_params.yaw_rate_max); + _params.yaw_ff = _params_handles.yaw_ff.update(); + _params.yaw_rate_max = math::radians(_params_handles.yaw_rate_max.update()); /* manual control scale */ - PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max); - PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max); - PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); + _params.man_roll_max = math::radians(_params_handles.man_roll_max.update()); + _params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update()); + _params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update()); /* acro control scale */ - PX4_PARAM_GET(_params_handles.acro_roll_max, &v); - _params.acro_rate_max(0) = math::radians(v); - PX4_PARAM_GET(_params_handles.acro_pitch_max, &v); - _params.acro_rate_max(1) = math::radians(v); - PX4_PARAM_GET(_params_handles.acro_yaw_max, &v); - _params.acro_rate_max(2) = math::radians(v); + _params.acro_rate_max(0) = math::radians(_params_handles.acro_roll_max.update()); + _params.acro_rate_max(1) = math::radians(_params_handles.acro_pitch_max.update()); + _params.acro_rate_max(2) = math::radians(_params_handles.acro_yaw_max.update()); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h index 95d608684a..e44317316d 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control.h +++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h @@ -95,29 +95,28 @@ private: px4::NodeHandle _n; struct { - px4_param_t roll_p; - px4_param_t roll_rate_p; - px4_param_t roll_rate_i; - px4_param_t roll_rate_d; - px4_param_t pitch_p; - px4_param_t pitch_rate_p; - px4_param_t pitch_rate_i; - px4_param_t pitch_rate_d; - px4_param_t yaw_p; - px4_param_t yaw_rate_p; - px4_param_t yaw_rate_i; - px4_param_t yaw_rate_d; - px4_param_t yaw_ff; - px4_param_t yaw_rate_max; + px4::ParameterFloat roll_p; + px4::ParameterFloat roll_rate_p; + px4::ParameterFloat roll_rate_i; + px4::ParameterFloat roll_rate_d; + px4::ParameterFloat pitch_p; + px4::ParameterFloat pitch_rate_p; + px4::ParameterFloat pitch_rate_i; + px4::ParameterFloat pitch_rate_d; + px4::ParameterFloat yaw_p; + px4::ParameterFloat yaw_rate_p; + px4::ParameterFloat yaw_rate_i; + px4::ParameterFloat yaw_rate_d; + px4::ParameterFloat yaw_ff; + px4::ParameterFloat yaw_rate_max; - px4_param_t man_roll_max; - px4_param_t man_pitch_max; - px4_param_t man_yaw_max; - px4_param_t acro_roll_max; - px4_param_t acro_pitch_max; - px4_param_t acro_yaw_max; + px4::ParameterFloat man_roll_max; + px4::ParameterFloat man_pitch_max; + px4::ParameterFloat man_yaw_max; + px4::ParameterFloat acro_roll_max; + px4::ParameterFloat acro_pitch_max; + px4::ParameterFloat acro_yaw_max; - px4_param_t autostart_id; } _params_handles; /**< handles for interesting parameters */ perf_counter_t _loop_perf; /**< loop performance counter */ From 27f2eb8a16fc6915783276298ecf676b8649c1f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 17:34:30 +0100 Subject: [PATCH 10/12] mc pos ctrl multiplatform use new param api --- .../mc_pos_control.cpp | 106 ++++++++---------- .../mc_pos_control.h | 35 +++--- .../mc_pos_control_multiplatform/module.mk | 2 + 3 files changed, 65 insertions(+), 78 deletions(-) diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index b7fce50294..391558dcce 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -52,11 +52,38 @@ MulticopterPositionControl::MulticopterPositionControl() : _control_task(-1), _mavlink_fd(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), + /* outgoing messages */ + _att_sp_msg(), + _local_pos_sp_msg(), + _global_vel_sp_msg(), + + _n(), + + /* parameters */ + _params_handles({ + .thr_min = px4::ParameterFloat("MPC_THR_MIN", PARAM_MPC_THR_MIN_DEFAULT), + .thr_max = px4::ParameterFloat("MPC_THR_MAX", PARAM_MPC_THR_MAX_DEFAULT), + .z_p = px4::ParameterFloat("MPC_Z_P", PARAM_MPC_Z_P_DEFAULT), + .z_vel_p = px4::ParameterFloat("MPC_Z_VEL_P", PARAM_MPC_Z_VEL_P_DEFAULT), + .z_vel_i = px4::ParameterFloat("MPC_Z_VEL_I", PARAM_MPC_Z_VEL_I_DEFAULT), + .z_vel_d = px4::ParameterFloat("MPC_Z_VEL_D", PARAM_MPC_Z_VEL_D_DEFAULT), + .z_vel_max = px4::ParameterFloat("MPC_Z_VEL_MAX", PARAM_MPC_Z_VEL_MAX_DEFAULT), + .z_ff = px4::ParameterFloat("MPC_Z_FF", PARAM_MPC_Z_FF_DEFAULT), + .xy_p = px4::ParameterFloat("MPC_XY_P", PARAM_MPC_XY_P_DEFAULT), + .xy_vel_p = px4::ParameterFloat("MPC_XY_VEL_P", PARAM_MPC_XY_VEL_P_DEFAULT), + .xy_vel_i = px4::ParameterFloat("MPC_XY_VEL_I", PARAM_MPC_XY_VEL_I_DEFAULT), + .xy_vel_d = px4::ParameterFloat("MPC_XY_VEL_D", PARAM_MPC_XY_VEL_D_DEFAULT), + .xy_vel_max = px4::ParameterFloat("MPC_XY_VEL_MAX", PARAM_MPC_XY_VEL_MAX_DEFAULT), + .xy_ff = px4::ParameterFloat("MPC_XY_FF", PARAM_MPC_XY_FF_DEFAULT), + .tilt_max_air = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT), + .land_speed = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT), + .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT) + }), _ref_alt(0.0f), _ref_timestamp(0), @@ -84,7 +111,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _global_vel_sp = _n.subscribe(0); - _params.pos_p.zero(); _params.vel_p.zero(); _params.vel_i.zero(); @@ -101,24 +127,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_ff.zero(); _sp_move_rate.zero(); - _params_handles.thr_min = PX4_PARAM_INIT(MPC_THR_MIN); - _params_handles.thr_max = PX4_PARAM_INIT(MPC_THR_MAX); - _params_handles.z_p = PX4_PARAM_INIT(MPC_Z_P); - _params_handles.z_vel_p = PX4_PARAM_INIT(MPC_Z_VEL_P); - _params_handles.z_vel_i = PX4_PARAM_INIT(MPC_Z_VEL_I); - _params_handles.z_vel_d = PX4_PARAM_INIT(MPC_Z_VEL_D); - _params_handles.z_vel_max = PX4_PARAM_INIT(MPC_Z_VEL_MAX); - _params_handles.z_ff = PX4_PARAM_INIT(MPC_Z_FF); - _params_handles.xy_p = PX4_PARAM_INIT(MPC_XY_P); - _params_handles.xy_vel_p = PX4_PARAM_INIT(MPC_XY_VEL_P); - _params_handles.xy_vel_i = PX4_PARAM_INIT(MPC_XY_VEL_I); - _params_handles.xy_vel_d = PX4_PARAM_INIT(MPC_XY_VEL_D); - _params_handles.xy_vel_max = PX4_PARAM_INIT(MPC_XY_VEL_MAX); - _params_handles.xy_ff = PX4_PARAM_INIT(MPC_XY_FF); - _params_handles.tilt_max_air = PX4_PARAM_INIT(MPC_TILTMAX_AIR); - _params_handles.land_speed = PX4_PARAM_INIT(MPC_LAND_SPEED); - _params_handles.tilt_max_land = PX4_PARAM_INIT(MPC_TILTMAX_LND); - /* fetch initial parameter values */ parameters_update(); @@ -132,47 +140,25 @@ MulticopterPositionControl::~MulticopterPositionControl() int MulticopterPositionControl::parameters_update() { - PX4_PARAM_GET(_params_handles.thr_min, &_params.thr_min); - PX4_PARAM_GET(_params_handles.thr_max, &_params.thr_max); - PX4_PARAM_GET(_params_handles.tilt_max_air, &_params.tilt_max_air); - _params.tilt_max_air = math::radians(_params.tilt_max_air); - PX4_PARAM_GET(_params_handles.land_speed, &_params.land_speed); - PX4_PARAM_GET(_params_handles.tilt_max_land, &_params.tilt_max_land); - _params.tilt_max_land = math::radians(_params.tilt_max_land); + _params.thr_min = _params_handles.thr_min.update(); + _params.thr_max = _params_handles.thr_max.update(); + _params.tilt_max_air = math::radians(_params_handles.tilt_max_air.update()); + _params.land_speed = _params_handles.land_speed.update(); + _params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update()); - float v; - PX4_PARAM_GET(_params_handles.xy_p, &v); - _params.pos_p(0) = v; - _params.pos_p(1) = v; - PX4_PARAM_GET(_params_handles.z_p, &v); - _params.pos_p(2) = v; - PX4_PARAM_GET(_params_handles.xy_vel_p, &v); - _params.vel_p(0) = v; - _params.vel_p(1) = v; - PX4_PARAM_GET(_params_handles.z_vel_p, &v); - _params.vel_p(2) = v; - PX4_PARAM_GET(_params_handles.xy_vel_i, &v); - _params.vel_i(0) = v; - _params.vel_i(1) = v; - PX4_PARAM_GET(_params_handles.z_vel_i, &v); - _params.vel_i(2) = v; - PX4_PARAM_GET(_params_handles.xy_vel_d, &v); - _params.vel_d(0) = v; - _params.vel_d(1) = v; - PX4_PARAM_GET(_params_handles.z_vel_d, &v); - _params.vel_d(2) = v; - PX4_PARAM_GET(_params_handles.xy_vel_max, &v); - _params.vel_max(0) = v; - _params.vel_max(1) = v; - PX4_PARAM_GET(_params_handles.z_vel_max, &v); - _params.vel_max(2) = v; - PX4_PARAM_GET(_params_handles.xy_ff, &v); - v = math::constrain(v, 0.0f, 1.0f); - _params.vel_ff(0) = v; - _params.vel_ff(1) = v; - PX4_PARAM_GET(_params_handles.z_ff, &v); - v = math::constrain(v, 0.0f, 1.0f); - _params.vel_ff(2) = v; + _params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update(); + _params.pos_p(2) = _params_handles.z_p.update(); + _params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update(); + _params.vel_p(2) = _params_handles.z_vel_p.update(); + _params.vel_i(0) = _params.vel_i(1) = _params_handles.xy_vel_i.update(); + _params.vel_i(2) = _params_handles.z_vel_i.update(); + _params.vel_d(0) = _params.vel_d(1) = _params_handles.xy_vel_d.update(); + _params.vel_d(2) = _params_handles.z_vel_d.update(); + _params.vel_max(0) = _params.vel_max(1) = _params_handles.xy_vel_max.update(); + _params.vel_max(2) = _params_handles.z_vel_max.update(); + + _params.vel_ff(0) = _params.vel_ff(1) = math::constrain(_params_handles.xy_ff.update(), 0.0f, 1.0f); + _params.vel_ff(2) = math::constrain(_params_handles.z_ff.update(), 0.0f, 1.0f); _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h index 05bd1387be..245b5f5a91 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h @@ -107,25 +107,24 @@ protected: px4::NodeHandle _n; - struct { - px4_param_t thr_min; - px4_param_t thr_max; - px4_param_t z_p; - px4_param_t z_vel_p; - px4_param_t z_vel_i; - px4_param_t z_vel_d; - px4_param_t z_vel_max; - px4_param_t z_ff; - px4_param_t xy_p; - px4_param_t xy_vel_p; - px4_param_t xy_vel_i; - px4_param_t xy_vel_d; - px4_param_t xy_vel_max; - px4_param_t xy_ff; - px4_param_t tilt_max_air; - px4_param_t land_speed; - px4_param_t tilt_max_land; + px4::ParameterFloat thr_min; + px4::ParameterFloat thr_max; + px4::ParameterFloat z_p; + px4::ParameterFloat z_vel_p; + px4::ParameterFloat z_vel_i; + px4::ParameterFloat z_vel_d; + px4::ParameterFloat z_vel_max; + px4::ParameterFloat z_ff; + px4::ParameterFloat xy_p; + px4::ParameterFloat xy_vel_p; + px4::ParameterFloat xy_vel_i; + px4::ParameterFloat xy_vel_d; + px4::ParameterFloat xy_vel_max; + px4::ParameterFloat xy_ff; + px4::ParameterFloat tilt_max_air; + px4::ParameterFloat land_speed; + px4::ParameterFloat tilt_max_land; } _params_handles; /**< handles for interesting parameters */ struct { diff --git a/src/modules/mc_pos_control_multiplatform/module.mk b/src/modules/mc_pos_control_multiplatform/module.mk index 2852ebbec8..9f7548e8a2 100644 --- a/src/modules/mc_pos_control_multiplatform/module.mk +++ b/src/modules/mc_pos_control_multiplatform/module.mk @@ -41,3 +41,5 @@ SRCS = mc_pos_control_main.cpp \ mc_pos_control_start_nuttx.cpp \ mc_pos_control.cpp \ mc_pos_control_params.c + +EXTRACXXFLAGS = -Wframe-larger-than=1040 From d1e14741cd61abcb6c69b35d73a6510513a20cab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 17:45:29 +0100 Subject: [PATCH 11/12] mixer node: do not use macros This is not a portable module (ros only). Hence use the native ros types. --- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 0749c8e926..1098ec73b0 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -55,8 +55,8 @@ public: float yaw_scale; }; - void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); - void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg); + void actuatorControlsCallback(const px4::actuator_controls_0 &msg); + void actuatorArmedCallback(const px4::actuator_armed &msg); private: @@ -211,7 +211,7 @@ void MultirotorMixer::mix() } } -void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &msg) { // read message for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) { @@ -266,7 +266,7 @@ int main(int argc, char **argv) return 0; } -void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) +void MultirotorMixer::actuatorArmedCallback(const px4::actuator_armed &msg) { _armed = msg.armed; } From 347047bafcbe9aa4268f4fad83d8b35a8787eda8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Feb 2015 17:52:18 +0100 Subject: [PATCH 12/12] get rid of lots of multiplatform macros, oh yeah --- src/platforms/px4_defines.h | 74 +------------------------------------ 1 file changed, 2 insertions(+), 72 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index fa4e1398e2..996e56efb9 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -54,6 +54,7 @@ #ifdef __cplusplus #include "ros/ros.h" #endif + /* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) @@ -61,48 +62,8 @@ #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO -/* Topic Handle */ -#define PX4_TOPIC(_name) #_name - -/* Topic type */ -#define PX4_TOPIC_T(_name) px4::_name - -/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr); -/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); - -/* Parameter handle datatype */ -typedef const char *px4_param_t; - -/* Helper functions to set ROS params, only int and float supported */ -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) -{ - if (!ros::param::has(name)) { - ros::param::set(name, value); - } - - return (px4_param_t)name; -}; -static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) -{ - if (!ros::param::has(name)) { - ros::param::set(name, value); - } - - return (px4_param_t)name; -}; - -/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ -#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) - -/* Get value of parameter by handle */ -#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) - /* Get value of parameter by name, which is equal to the handle for ros */ -#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt) +#define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt) #define OK 0 #define ERROR -1 @@ -150,29 +111,10 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) #define PX4_WARN warnx #define PX4_INFO warnx -/* Topic Handle */ -#define PX4_TOPIC(_name) ORB_ID(_name) - -/* Topic type */ -#define PX4_TOPIC_T(_name) _name##_s - -/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval) -/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _interval) - /* Parameter handle datatype */ #include typedef param_t px4_param_t; -/* Initialize a param, get param handle */ -#define PX4_PARAM_INIT(_name) param_find(#_name) - -/* Get value of parameter by handle */ -#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) - /* Get value of parameter by name */ #define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt) @@ -187,18 +129,6 @@ typedef param_t px4_param_t; /* Defines for all platforms */ -/* Shortcut for subscribing to topics - * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all - */ -#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME -#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__) - -/* Get a subscriber class type based on the topic name */ -#define PX4_SUBSCRIBER(_name) Subscriber - -/* shortcut for advertising topics */ -#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise(PX4_TOPIC(_name)) - /* wrapper for 2d matrices */ #define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y])