dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc att: use PX4 macros to init and get params

This commit is contained in:
Thomas Gubler 2014-12-11 17:12:03 +01:00
parent 9d0ecc77bb
commit b755312a1e
2 changed files with 60 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/modules/mc_att_control/mc_att_control.cpp
View File

@ -83,26 +83,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{ {
_params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_p = PX4_PARAM_INIT("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_p = PX4_PARAM_INIT("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_i = PX4_PARAM_INIT("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); _params_handles.roll_rate_d = PX4_PARAM_INIT("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_p = PX4_PARAM_INIT("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_p = PX4_PARAM_INIT("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_i = PX4_PARAM_INIT("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); _params_handles.pitch_rate_d = PX4_PARAM_INIT("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_p = PX4_PARAM_INIT("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_p = PX4_PARAM_INIT("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_i = PX4_PARAM_INIT("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); _params_handles.yaw_rate_d = PX4_PARAM_INIT("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_ff = PX4_PARAM_INIT("MC_YAW_FF");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.yaw_rate_max = PX4_PARAM_INIT("MC_YAWRATE_MAX");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_roll_max = PX4_PARAM_INIT("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_pitch_max = PX4_PARAM_INIT("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); _params_handles.man_yaw_max = PX4_PARAM_INIT("MC_MAN_Y_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_roll_max = PX4_PARAM_INIT("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_pitch_max = PX4_PARAM_INIT("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.acro_yaw_max = PX4_PARAM_INIT("MC_ACRO_Y_MAX");
/* fetch initial parameter values */ /* fetch initial parameter values */
parameters_update(); parameters_update();
@ -157,53 +157,53 @@ MulticopterAttitudeControl::parameters_update()
float v; float v;
/* roll gains */ /* roll gains */
param_get(_params_handles.roll_p, &v); PX4_PARAM_GET(_params_handles.roll_p, &v);
_params.att_p(0) = v; _params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v); PX4_PARAM_GET(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v; _params.rate_p(0) = v;
param_get(_params_handles.roll_rate_i, &v); PX4_PARAM_GET(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v; _params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v); PX4_PARAM_GET(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v; _params.rate_d(0) = v;
/* pitch gains */ /* pitch gains */
param_get(_params_handles.pitch_p, &v); PX4_PARAM_GET(_params_handles.pitch_p, &v);
_params.att_p(1) = v; _params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v); PX4_PARAM_GET(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v; _params.rate_p(1) = v;
param_get(_params_handles.pitch_rate_i, &v); PX4_PARAM_GET(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v; _params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v); PX4_PARAM_GET(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v; _params.rate_d(1) = v;
/* yaw gains */ /* yaw gains */
param_get(_params_handles.yaw_p, &v); PX4_PARAM_GET(_params_handles.yaw_p, &v);
_params.att_p(2) = v; _params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v); PX4_PARAM_GET(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v; _params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v); PX4_PARAM_GET(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v; _params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v); PX4_PARAM_GET(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v; _params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff); PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.yaw_rate_max = math::radians(_params.yaw_rate_max); _params.yaw_rate_max = math::radians(_params.yaw_rate_max);
/* manual control scale */ /* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max); PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_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_roll_max = math::radians(_params.man_roll_max);
_params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_pitch_max = math::radians(_params.man_pitch_max);
_params.man_yaw_max = math::radians(_params.man_yaw_max); _params.man_yaw_max = math::radians(_params.man_yaw_max);
/* acro control scale */ /* acro control scale */
param_get(_params_handles.acro_roll_max, &v); PX4_PARAM_GET(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v); _params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v); PX4_PARAM_GET(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v); _params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v); PX4_PARAM_GET(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v); _params.acro_rate_max(2) = math::radians(v);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);

40
src/modules/mc_att_control/mc_att_control.h
View File

@ -110,27 +110,27 @@ private:
px4::NodeHandle n; px4::NodeHandle n;
struct { struct {
param_t roll_p; px4_param_t roll_p;
param_t roll_rate_p; px4_param_t roll_rate_p;
param_t roll_rate_i; px4_param_t roll_rate_i;
param_t roll_rate_d; px4_param_t roll_rate_d;
param_t pitch_p; px4_param_t pitch_p;
param_t pitch_rate_p; px4_param_t pitch_rate_p;
param_t pitch_rate_i; px4_param_t pitch_rate_i;
param_t pitch_rate_d; px4_param_t pitch_rate_d;
param_t yaw_p; px4_param_t yaw_p;
param_t yaw_rate_p; px4_param_t yaw_rate_p;
param_t yaw_rate_i; px4_param_t yaw_rate_i;
param_t yaw_rate_d; px4_param_t yaw_rate_d;
param_t yaw_ff; px4_param_t yaw_ff;
param_t yaw_rate_max; px4_param_t yaw_rate_max;
param_t man_roll_max; px4_param_t man_roll_max;
param_t man_pitch_max; px4_param_t man_pitch_max;
param_t man_yaw_max; px4_param_t man_yaw_max;
param_t acro_roll_max; px4_param_t acro_roll_max;
param_t acro_pitch_max; px4_param_t acro_pitch_max;
param_t acro_yaw_max; px4_param_t acro_yaw_max;
} _params_handles; /**< handles for interesting parameters */ } _params_handles; /**< handles for interesting parameters */
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */