From f511d3a399e92f88fca1e67156010698b81c9749 Mon Sep 17 00:00:00 2001 From: Martina Date: Thu, 5 Apr 2018 15:04:24 +0200 Subject: [PATCH] mc_pos_control: add method to constrain velocity setpoint --- .../mc_pos_control/mc_pos_control_main.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 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 26a897100d..69bbc70f9b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -381,6 +381,8 @@ private: float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next); + void constrain_velocity_setpoint(); + void set_manual_acceleration_xy(matrix::Vector2f &stick_input_xy_NED); void set_manual_acceleration_z(float &max_acc_z, const float stick_input_z_NED); @@ -965,6 +967,22 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c } +void +MulticopterPositionControl::constrain_velocity_setpoint() +{ + + /* make sure velocity setpoint is constrained in all directions (xyz) */ + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _vel_max_xy) { + _vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy; + } + + _vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get()); + +} + float MulticopterPositionControl::get_cruising_speed_xy() { @@ -2461,19 +2479,13 @@ MulticopterPositionControl::calculate_velocity_setpoint() set_takeoff_velocity(_vel_sp(2)); } + constrain_velocity_setpoint(); /* make sure velocity setpoint is constrained in all directions (xyz) */ float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); /* check if the velocity demand is significant */ _vel_sp_significant = vel_norm_xy > 0.5f * _vel_max_xy; - if (vel_norm_xy > _vel_max_xy) { - _vel_sp(0) = _vel_sp(0) * _vel_max_xy / vel_norm_xy; - _vel_sp(1) = _vel_sp(1) * _vel_max_xy / vel_norm_xy; - } - - _vel_sp(2) = math::constrain(_vel_sp(2), -_vel_max_up.get(), _vel_max_down.get()); - _vel_sp_prev = _vel_sp; }