LP_616 Bugfix for pathplanner > Circle Left/Right > End Condition > Pointing Towards Next
Julian Lilov
Branch: julian_lilov/librepilot_jdl:LP-616_pp_fix
Branch: librepilot/librepilot:next
Merged
Merged pull request
Merged in LP-616_pp_fix (pull request #539)
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:
PositionStateData positionState; PositionStateGet(&positionState); // current aircraft position
Use it for angle1 calculation:
float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East));
Â