mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Merged in LP-616_pp_fix (pull request #539)
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:
commit
42c14a2a0b
@ -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);
|
||||
@ -662,7 +675,15 @@ static uint8_t conditionPointingTowardsNext()
|
||||
WaypointData 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;
|
||||
VelocityStateGet(&velocity);
|
||||
|
Loading…
x
Reference in New Issue
Block a user