1. LibrePilot
  2. LibrePilot
  3. LibrePilot
  4. Pull requests

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

Open
#539 · Created  · Last updated

Description

The vector pointing towards next waypoint is being incorrecly constructed. Current waypoint position (in fact, this is the center of the circle) is used for start point and the next waypoint position as end point of the vector. This is incorrect, the start point should be the current aircraft position.

The fix:
static uint8_t conditionPointingTowardsNext()

Get current aircraft position:

1 2 PositionStateData positionState; PositionStateGet(&positionState); // current aircraft position

Use it for angle1 calculation:

1 float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East));

 

0 attachments

Loading commits...