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

Yaw mode 3 - Add check for minimum distance to target in MPC_YAW_MODE 3 (along trajectory) to be consistent with the other yaw modes

Browse Source
This commit is contained in:
bresch
2019-08-07 10:30:34 +02:00
committed by Roman Bapst
parent 119e5e3182
commit 50fbb56737
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+4 -1
View File
@@ -121,9 +121,12 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
{
bool res = false;
Vector2f vel_sp_xy(_velocity_setpoint);
Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position);
if (vel_sp_xy.length() > .1f) {
if ((vel_sp_xy.length() > .1f) &&
(traj_to_target.length() > _target_acceptance_radius)) {
// Generate heading from velocity vector, only if it is long enough
// and if the drone is far enough from the target
_compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy);
res = true;
}