fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:28:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: use original targethreshold when computing target_velocity

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 08d15f5402
commit cb820a168a
1 changed files with 2 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+2 -1
View File
@@ -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;
}