diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-02-10 14:12:23 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-02-10 14:12:23 +0100 |
commit | 7c29a58ba61c9db63cdf0ad09ab443f4731198c2 (patch) | |
tree | a46140b6425f34515241b9754050524b40d2ca9c | |
parent | 38ad35fd7729d9492aa5ae7b5186d4a4676d5df2 (diff) | |
parent | 347047bafcbe9aa4268f4fad83d8b35a8787eda8 (diff) | |
download | px4-firmware-7c29a58ba61c9db63cdf0ad09ab443f4731198c2.tar.gz px4-firmware-7c29a58ba61c9db63cdf0ad09ab443f4731198c2.tar.bz2 px4-firmware-7c29a58ba61c9db63cdf0ad09ab443f4731198c2.zip |
Merge pull request #1763 from PX4/ros_paramapi
multiplatform: improve param api
-rw-r--r-- | src/examples/subscriber/subscriber_example.cpp | 24 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.h | 7 | ||||
-rw-r--r-- | src/include/px4.h | 1 | ||||
-rw-r--r-- | src/modules/mc_att_control_multiplatform/mc_att_control.cpp | 109 | ||||
-rw-r--r-- | src/modules/mc_att_control_multiplatform/mc_att_control.h | 45 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 108 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.h | 35 | ||||
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/module.mk | 2 | ||||
-rw-r--r-- | src/platforms/px4_defines.h | 74 | ||||
-rw-r--r-- | src/platforms/px4_parameter.h | 161 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 4 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 2 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 8 |
13 files changed, 318 insertions, 262 deletions
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index e1292f788..496e1dcd8 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<px4_rc_channels>(rc_channels_callback_function, _interval); + _n.subscribe<px4_rc_channels>(rc_channels_callback_function, _p_sub_interv.get()); /* No callback */ _sub_rc_chan = _n.subscribe<px4_rc_channels>(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 9b6d890e3..686fed023 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<px4_rc_channels> * _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); diff --git a/src/include/px4.h b/src/include/px4.h index 34137ee6f..b93ab5f80 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/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp index 1e40c2b05..ecbf98c9e 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<px4_manual_control_setpoint>(0); _armed = _n.subscribe<px4_actuator_armed>(0); _v_status = _n.subscribe<px4_vehicle_status>(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; - - 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.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(); + + _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 95d608684..e44317316 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_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_param_t autostart_id; + 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::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; + } _params_handles; /**< handles for interesting parameters */ perf_counter_t _loop_perf; /**< loop performance counter */ 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 b7fce5029..391558dcc 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<px4_vehicle_global_velocity_setpoint>(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); - - 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.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()); + + _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 05bd1387b..245b5f5a9 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 2852ebbec..9f7548e8a 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 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index fa4e1398e..996e56efb 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_T(_name)>(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_T(_name)>(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_T(_name)>(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_T(_name)>(PX4_TOPIC(_name), _interval) - /* Parameter handle datatype */ #include <systemlib/param/param.h> 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<PX4_TOPIC_T(_name)> - -/* shortcut for advertising topics */ -#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name)) - /* wrapper for 2d matrices */ #define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y]) diff --git a/src/platforms/px4_parameter.h b/src/platforms/px4_parameter.h new file mode 100644 index 000000000..1376320b1 --- /dev/null +++ b/src/platforms/px4_parameter.h @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * 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 <string> +#else +/* includes when building for NuttX */ +#include <systemlib/param/param.h> +#endif + +namespace px4 +{ + +/** + * Parameter base class + */ +template <typename T> +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 <typename T> +class Parameter : + public ParameterBase<T> +{ +public: + Parameter(const char *name, T value) : + ParameterBase<T>(name, value) + { + if (!ros::param::has(name)) { + ros::param::set(name, value); + } + + update(); + } + + ~Parameter() {}; + + /** + * Update the parameter value + */ + T update() + { + ros::param::get(this->_name, this->_value); + return get(); + } + + /** + * Get the current parameter value + */ + T get() + { + return this->_value; + } + +}; +#else +template <typename T> +class __EXPORT Parameter : + public ParameterBase<T> +{ +public: + Parameter(const char *name, T value) : + ParameterBase<T>(name, value), + _handle(param_find(name)) + { + update(); + } + + ~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<float> ParameterFloat; +typedef Parameter<int> ParameterInt; +} diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index d9cd7a3c1..eb8e964e7 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 6ca35b173..9b0ca1319 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 diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 0749c8e92..1098ec73b 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; } |