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

Merged
#539 · Created  · Last updated

Merged pull request

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

42c14a2·Author: ·Closed by: ·2022-02-16

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:

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));

 

0 attachments

0 comments

Loading commits...