From b769d1cee04083142825cb2b12138ecea839bc54 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 12 Jun 2017 13:58:24 +0200 Subject: [PATCH] mc_pos_control auto: apply auto logic for z-direction --- .../mc_pos_control/mc_pos_control_main.cpp | 87 +++++++++++++++++++ 1 file changed, 87 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 7f557ffa66..f87d127639 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1492,6 +1492,93 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp = _curr_pos_sp; + /* + * Z-DIRECTION + */ + + /* get various distances */ + float total_dist_z = fabsf(_curr_pos_sp(2) - _prev_pos_sp(2)); + float dist_to_prev_z = fabsf(_pos(2) - _prev_pos_sp(2)); + float dist_to_current_z = fabsf(_curr_pos_sp(2) - _pos(2)); + + /* if pos_sp has not reached target setpoint (=curr_pos_sp(2)), + * then compute setpoint depending on vel_max */ + if ((total_dist_z > SIGMA_NORM) && (fabsf(_pos_sp(2) - _curr_pos_sp(2)) > SIGMA_NORM)) { + + /* check sign */ + bool flying_upward = _curr_pos_sp(2) < _prev_pos_sp(2); + + /* final_vel_z is the max velocity which depends on the distance of total_dist_z + * with default params.vel_max_up/down + */ + float final_vel_z = (flying_upward) ? _params.vel_max_up : _params.vel_max_down; + + /* target threshold defines the distance to _curr_pos_sp(2) at which + * the vehicle starts to slow down to approach the target smoothly + */ + float target_threshold_z = final_vel_z * 1.5f; + + /* if the total distance in z is NOT 2x distance of target_threshold, we + * will need to adjust the final_vel_z + */ + bool is_2_target_threshold_z = total_dist_z >= 2.0f * target_threshold_z; + float slope = (final_vel_z) / (target_threshold_z); /* defines the the acceleration when slowing down */ + float min_vel_z = 0.2f; // minimum velocity: this is needed since estimation is not perfect + + if (!is_2_target_threshold_z) { + /* adjust final_vel_z since we are already very close + * to current and therefore it is not necessary to accelerate + * up to full speed (=final_vel_z) + */ + + target_threshold_z = total_dist_z * 0.5f; + /* get the velocity at target_threshold_z */ + float final_vel_z_tmp = slope * (target_threshold_z) + min_vel_z; + + /* make sure that final_vel_z is never smaller than 0.5 of the default final_vel_z + * this is mainly done because the estimation in z is not perfect and therefore + * it is necessary to have a minimum speed + */ + final_vel_z = math::constrain(final_vel_z_tmp, final_vel_z * 0.5f, final_vel_z); + } + + float vel_sp_z = final_vel_z; + + /* we want to slow down */ + if (dist_to_current_z < target_threshold_z) { + + vel_sp_z = slope * dist_to_current_z + min_vel_z; + + } else if (dist_to_prev_z < target_threshold_z) { + /* we want to accelerate */ + + float acc_z = fabsf(vel_sp_z - _vel_sp_prev(2)) / dt; + + if (acc_z > _acceleration_z_max_up.get()) { + vel_sp_z = _acceleration_z_max_up.get() * dt + fabsf(_vel_sp_prev(2)); + } + } + + /* if we already close to current, then just take over the velocity that + * we would have computed if going directly to the current setpoint + */ + if (vel_sp_z >= (dist_to_current_z * _params.pos_p(2))) { + vel_sp_z = dist_to_current_z * _params.pos_p(2); + } + + /* make sure vel_sp_z is always positive */ + vel_sp_z = math::constrain(vel_sp_z, 0.0f, final_vel_z); + /* get the sign of vel_sp_z */ + vel_sp_z = (flying_upward) ? -vel_sp_z : vel_sp_z; + /* compute pos_sp(2) */ + pos_sp(2) = _pos(2) + vel_sp_z / _params.pos_p(2); + + } + + /* + * XY-DIRECTION + */ + /* line from previous to current and from pos to current */ matrix::Vector2f vec_prev_to_current((_curr_pos_sp(0) - _prev_pos_sp(0)), (_curr_pos_sp(1) - _prev_pos_sp(1))); matrix::Vector2f vec_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));