From b02c4ec3562c1713fee0bc4d4a55eddf22df63b0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 27 Jun 2015 18:12:18 +0200 Subject: [PATCH] add max vel constraints to multiplatform MPC --- .../mc_pos_control.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 e0e086999d..37775711e0 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -647,6 +647,21 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + /* 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->data().flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f;