aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mc_att_control
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2014-12-11 17:12:03 +0100
committerThomas Gubler <thomasgubler@gmail.com>2014-12-11 17:12:03 +0100
commitb755312a1ec3e1f73827ac2b581a597491601140 (patch)
tree4b8fc35bebff261016143561bb9e00cddbb78dd6 /src/modules/mc_att_control
parent9d0ecc77bb1c3d1c9b5cf6bc5d517e45d9f20f97 (diff)
downloadpx4-firmware-b755312a1ec3e1f73827ac2b581a597491601140.tar.gz
px4-firmware-b755312a1ec3e1f73827ac2b581a597491601140.tar.bz2
px4-firmware-b755312a1ec3e1f73827ac2b581a597491601140.zip
mc att: use PX4 macros to init and get params
Diffstat (limited to 'src/modules/mc_att_control')
-rw-r--r--src/modules/mc_att_control/mc_att_control.cpp80
-rw-r--r--src/modules/mc_att_control/mc_att_control.h42
2 files changed, 61 insertions, 61 deletions
diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 002812741..d20bf24e1 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -83,26 +83,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
- _params_handles.roll_p = param_find("MC_ROLL_P");
- _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
- _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
- _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
- _params_handles.pitch_p = param_find("MC_PITCH_P");
- _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
- _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
- _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
- _params_handles.yaw_p = param_find("MC_YAW_P");
- _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
- _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
- _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
- _params_handles.yaw_ff = param_find("MC_YAW_FF");
- _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
- _params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
- _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
- _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
- _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
- _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
- _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
+ _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();
@@ -157,53 +157,53 @@ MulticopterAttitudeControl::parameters_update()
float v;
/* roll gains */
- param_get(_params_handles.roll_p, &v);
+ PX4_PARAM_GET(_params_handles.roll_p, &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;
- param_get(_params_handles.roll_rate_i, &v);
+ PX4_PARAM_GET(_params_handles.roll_rate_i, &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;
/* pitch gains */
- param_get(_params_handles.pitch_p, &v);
+ PX4_PARAM_GET(_params_handles.pitch_p, &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;
- param_get(_params_handles.pitch_rate_i, &v);
+ PX4_PARAM_GET(_params_handles.pitch_rate_i, &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;
/* yaw gains */
- param_get(_params_handles.yaw_p, &v);
+ PX4_PARAM_GET(_params_handles.yaw_p, &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;
- param_get(_params_handles.yaw_rate_i, &v);
+ PX4_PARAM_GET(_params_handles.yaw_rate_i, &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;
- 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_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);
/* manual control scale */
- param_get(_params_handles.man_roll_max, &_params.man_roll_max);
- 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_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);
/* 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);
- 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);
- 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);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index b2ec3083f..4e1dd0b57 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -110,27 +110,27 @@ private:
px4::NodeHandle n;
struct {
- param_t roll_p;
- param_t roll_rate_p;
- param_t roll_rate_i;
- param_t roll_rate_d;
- param_t pitch_p;
- param_t pitch_rate_p;
- param_t pitch_rate_i;
- param_t pitch_rate_d;
- param_t yaw_p;
- param_t yaw_rate_p;
- param_t yaw_rate_i;
- param_t yaw_rate_d;
- param_t yaw_ff;
- param_t yaw_rate_max;
-
- param_t man_roll_max;
- param_t man_pitch_max;
- param_t man_yaw_max;
- param_t acro_roll_max;
- param_t acro_pitch_max;
- param_t acro_yaw_max;
+ 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;
} _params_handles; /**< handles for interesting parameters */
perf_counter_t _loop_perf; /**< loop performance counter */