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

mc pos control: More sign checks

Browse Source
This commit is contained in:
Lorenz Meier
2017-09-11 02:08:22 -07:00
parent a6108cc951
commit 38f796f991
1 changed files with 3 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+3
View File
@@ -1579,6 +1579,9 @@ void MulticopterPositionControl::control_auto(float dt)
float slope = (get_cruising_speed_xy() - vel_close) / (_target_threshold_xy.get() - _nav_rad.get()) ;
vel_sp_along_track = slope * (vec_closest_to_current.length()) + vel_close;
/* make sure vel_sp_along_track is positive */
vel_sp_along_track = (vel_sp_along_track < 0.0f) ? SIGMA_SINGLE_OP : vel_sp_along_track;
/* since we want to slow down take over previous velocity setpoint along track if it was lower */
if ((vel_sp_along_track_prev < vel_sp_along_track) && (vel_sp_along_track * vel_sp_along_track_prev > 0.0f)) {
vel_sp_along_track = vel_sp_along_track_prev;