From cb820a168a6f9917a6bd3ef6bdb6fc61150bd9c3 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 12 Jun 2017 14:21:42 +0200 Subject: [PATCH] mc_pos_control: use original targethreshold when computing target_velocity --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 c43347d6f9..fd9a137584 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1580,6 +1580,7 @@ void MulticopterPositionControl::control_auto(float dt) if (!is_2_target_threshold) { /* set target threshold to half dist pre-current */ + float target_threshold_tmp = target_threshold_xy; target_threshold_xy = vec_prev_to_current.length() * 0.5f; if ((target_threshold_xy - _nav_rad.get()) < SIGMA_NORM) { @@ -1602,7 +1603,7 @@ void MulticopterPositionControl::control_auto(float dt) final_cruise_speed = vel_close; } else { - float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_xy - acceptance_radius); + float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_tmp - acceptance_radius); final_cruise_speed = slope * (target_threshold_xy - acceptance_radius) + vel_close; final_cruise_speed = (final_cruise_speed > vel_close) ? final_cruise_speed : vel_close; }