forked from Archive/PX4-Autopilot
Merge pull request #1763 from PX4/ros_paramapi
multiplatform: improve param api
This commit is contained in:
Lorenz Meier
commit
7c29a58ba6
|
|
@ -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());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
_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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
_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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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])
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
/**
|
||||
* @file px4_subscriber.h
|
||||
*
|
||||
* PX4 Middleware Wrapper Subscriber
|
||||
* PX4 Subscriber API, implements subscribing to messages from a nodehandle
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue