rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

Merged in LP-616_pp_fix (pull request #539)

Browse Source 
LP_616 Bugfix for pathplanner > Circle Left/Right > End Condition > Pointing Towards Next

Approved-by: Lalanne Laurent
Approved-by: Eric Price
This commit is contained in:
Julian Lilov 2022-02-16 01:23:58 +00:00 committed by Eric Price
commit 42c14a2a0b
1 changed files with 26 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -339,10 +339,23 @@ void updatePathDesired()
WaypointData waypointPrev; WaypointData waypointPrev;
WaypointInstGet(waypointActive.Index - 1, &waypointPrev); WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
pathDesired.Start.North = waypointPrev.Position.North; // When exiting CIRCLE, use current UAV position as a start point for PathDesired vector to the next waypoint
pathDesired.Start.East = waypointPrev.Position.East; // instead of previous waypoint location (that represents the center of the circle)
pathDesired.Start.Down = waypointPrev.Position.Down; PathActionData prevAction;
pathDesired.StartingVelocity = waypointPrev.Velocity; 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); PathDesiredSet(&pathDesired);
@ -662,7 +675,15 @@ static uint8_t conditionPointingTowardsNext()
WaypointData nextWaypoint; WaypointData nextWaypoint;
WaypointInstGet(nextWaypointId, &nextWaypoint); WaypointInstGet(nextWaypointId, &nextWaypoint);
float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East)); PositionStateData positionState;
PositionStateGet(&positionState);
// check if current position exactly matches nextWaipoint (in 2D space)
if ((fabsf(nextWaypoint.Position.North - positionState.North) < 1e-6f) && (fabsf(nextWaypoint.Position.East - positionState.East) < 1e-6f)) {
return true;
}
float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East));
VelocityStateData velocity; VelocityStateData velocity;
VelocityStateGet(&velocity); VelocityStateGet(&velocity);