From 8b20c66cfccea1d3a495c5888de04dc0005e731a Mon Sep 17 00:00:00 2001 From: Martina Date: Fri, 6 Apr 2018 15:38:16 +0200 Subject: [PATCH] mc_pos_control: use desired velocity to calculate vel_sp_along_track_prev if obstacle avoidance is running --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++ 1 file changed, 4 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 ffcca5c65c..f04ae65be1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -2021,6 +2021,10 @@ void MulticopterPositionControl::control_auto() /* current velocity along track */ float vel_sp_along_track_prev = matrix::Vector2f(_vel_sp(0), _vel_sp(1)) * unit_prev_to_current; + if (use_obstacle_avoidance()) { + vel_sp_along_track_prev = matrix::Vector2f(&_vel_sp_desired(0)) * unit_prev_to_current; + } + /* distance to target when brake should occur */ float target_threshold_xy = 1.5f * get_cruising_speed_xy();