diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-22 13:02:22 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-24 20:33:32 +0100 |
commit | a56c16dfab948517007ad570d67f02dd4e2edb60 (patch) | |
tree | 724c2534edbbaba37271a3edc63248baa558fe9d /src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | |
parent | f1c6b24f7a80f88baf272bc0b947f7e9649bee27 (diff) | |
download | px4-firmware-a56c16dfab948517007ad570d67f02dd4e2edb60.tar.gz px4-firmware-a56c16dfab948517007ad570d67f02dd4e2edb60.tar.bz2 px4-firmware-a56c16dfab948517007ad570d67f02dd4e2edb60.zip |
multiplatform port of #1741: move params from att control to pos control
Diffstat (limited to 'src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp')
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 10 |
1 files changed, 9 insertions, 1 deletions
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index 1fe75b90c..1c0298a07 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -82,7 +82,10 @@ MulticopterPositionControl::MulticopterPositionControl() : .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) + .tilt_max_land = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT), + .man_roll_max = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT), + .man_pitch_max = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT), + .man_yaw_max = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT) }), _ref_alt(0.0f), _ref_timestamp(0), @@ -146,6 +149,11 @@ MulticopterPositionControl::parameters_update() _params.land_speed = _params_handles.land_speed.update(); _params.tilt_max_land = math::radians(_params_handles.tilt_max_land.update()); + /* manual control scale */ + _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()); + _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(); |