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

mc_pos_control: don't divide by zero

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent cb820a168a
commit 8e99c73f49
1 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+9 -3
View File
@@ -1599,7 +1599,7 @@ void MulticopterPositionControl::control_auto(float dt)
}
/* compute velocity at transition where vehicle switches from acceleration to deceleration */
if ((target_threshold_xy - acceptance_radius) < SIGMA_NORM) {
if ((target_threshold_tmp - acceptance_radius) < SIGMA_NORM) {
final_cruise_speed = vel_close;
} else {
@@ -1649,8 +1649,14 @@ void MulticopterPositionControl::control_auto(float dt)
vel_sp_along_track = vel_close;
} else {
float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_xy - _nav_rad.get()) ;
vel_sp_along_track = slope * (vec_closest_to_current.length() - _nav_rad.get()) + vel_close;
if (target_threshold_xy - _nav_rad.get() < SIGMA_NORM) {
vel_sp_along_track = vel_close;
} else {
float slope = (get_cruising_speed_xy() - vel_close) / (target_threshold_xy - _nav_rad.get()) ;
vel_sp_along_track = slope * (vec_closest_to_current.length() - _nav_rad.get()) + vel_close;
}
}
/* since we want to slow down take over previous velocity setpoint along track if it was lower */