fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1763 from PX4/ros_paramapi

Browse Source 
multiplatform: improve param api
This commit is contained in:
Lorenz Meier
2015-02-10 14:12:23 +01:00
parent 38ad35fd77 347047bafc
commit 7c29a58ba6
13 changed files with 314 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+44 -63
View File
@@ -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);
src/modules/mc_att_control_multiplatform/mc_att_control.h
+20 -21
View File
@@ -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 */
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+46 -60
View File
@@ -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;
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+17 -18
View File
@@ -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 {
src/modules/mc_pos_control_multiplatform/module.mk
+2
View File
@@ -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