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

mc_pos_control: limit if just xy distance close to target

Browse Source
This commit is contained in:
Dennis Mannhart
2017-03-22 15:21:34 +01:00
parent f9551c12de
commit 20a47a1a72
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
@@ -1500,8 +1500,9 @@ void MulticopterPositionControl::control_auto(float dt)
}
/* set velocity limit if close to current setpoint and no next setpoint available */
math::Vector<3> dist = _curr_pos_sp - _pos;
_limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
&& ((_curr_pos_sp - _pos).length() <= _target_threshold_xy.get());
&& (sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) <= _target_threshold_xy.get());
if (current_setpoint_valid &&
(_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {