diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-08 16:31:59 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-08 17:53:26 +0100 |
commit | 893beb64a023d736d838a79b2f6ee54951dc2a65 (patch) | |
tree | 339e102af70ae986481522edc02abbe10a3cdb67 /src/modules/mc_att_control_multiplatform | |
parent | fbcf2c8fb50af65f03192b759ef31b73297d6114 (diff) | |
download | px4-firmware-893beb64a023d736d838a79b2f6ee54951dc2a65.tar.gz px4-firmware-893beb64a023d736d838a79b2f6ee54951dc2a65.tar.bz2 px4-firmware-893beb64a023d736d838a79b2f6ee54951dc2a65.zip |
mc att ctrl multiplatform use new param api
Diffstat (limited to 'src/modules/mc_att_control_multiplatform')
-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 |
2 files changed, 67 insertions, 87 deletions
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 */ |