Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

MC pos control: realize when we are on the trajectory but passed the target waypoint

Browse Source
This commit is contained in:
Andreas Antener
2016-08-12 00:19:54 +02:00
committed by Lorenz Meier
parent 710a8e8f56
commit c2e0246650
1 changed files with 5 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
+5
View File
@@ -1127,6 +1127,11 @@ void MulticopterPositionControl::control_auto(float dt)
if (near) {
/* unit sphere crosses trajectory */
/* if copter is in front of curr waypoint, go directly to curr waypoint */
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
} else {
/* copter is too far from trajectory */