rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-28 06:24:10 +01:00
Code Issues Releases Activity

LP_616 updatePathDesired() fix: leaving the circle on tangent

Browse Source
This commit is contained in:
julian_lilov 2020-10-16 11:17:27 +03:00
parent 65459c7ba0
commit a17a153a1b
1 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
flight/modules/PathPlanner/pathplanner.c
View File

@ -339,10 +339,23 @@ void updatePathDesired()
WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start.North = waypointPrev.Position.North;
pathDesired.Start.East = waypointPrev.Position.East;
pathDesired.Start.Down = waypointPrev.Position.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
// When exiting CIRCLE, use current UAV position as a start point for PathDesired vector to the next waypoint
// instead of previous waypoint location (that represents the center of the circle)
PathActionData prevAction;
PathActionInstGet(waypointPrev.Action, &prevAction);
if ((prevAction.Mode == PATHACTION_MODE_CIRCLERIGHT) || (prevAction.Mode == PATHACTION_MODE_CIRCLELEFT)) {
PositionStateData positionState;
PositionStateGet(&positionState);
pathDesired.Start.North = positionState.North;
pathDesired.Start.East = positionState.East;
pathDesired.Start.Down = positionState.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
} else {
pathDesired.Start.North = waypointPrev.Position.North;
pathDesired.Start.East = waypointPrev.Position.East;
pathDesired.Start.Down = waypointPrev.Position.Down;
pathDesired.StartingVelocity = waypointPrev.Velocity;
}
}
PathDesiredSet(&pathDesired);