From adbccfaa1cd100609490b61f2081a0619b0a36c8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 09:32:02 -0400 Subject: [PATCH 1/2] Saturate velocity command for mc_pos_control. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7a3a5a679b..2509f2b8c4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,7 +1032,14 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; + /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + for (int i=0; i<3; i++) { + if (_vel_sp(i) > _params.vel_max(i)) { + _vel_sp(i) = _params.vel_max(i); + } else if (_vel_sp(i) < -_params.vel_max(i)) + _vel_sp(i) = -_params.vel_max(i); + } if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; From dedd16e36e4f0690f8662b93f2aa8144cc8a57bf Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 21:15:17 -0400 Subject: [PATCH 2/2] Modified velocity saturation to maintain direction. --- .../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2509f2b8c4..995937aa6c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,13 +1032,21 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; - for (int i=0; i<3; i++) { - if (_vel_sp(i) > _params.vel_max(i)) { - _vel_sp(i) = _params.vel_max(i); - } else if (_vel_sp(i) < -_params.vel_max(i)) - _vel_sp(i) = -_params.vel_max(i); + + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } if (!_control_mode.flag_control_altitude_enabled) {