...
TODO: Do the math manually to see how the calculation is done (bruh i hate math)
Quick Overview About Waypoint Manager Math
Waypoint manager math breaks down into three big chunks: Straight path following, orbit following, and blending of both.
Straight path following
Given two points, get a line in-between.
If given XY direction, calculate the line’s direction using trigonometry.
If given GPS coordinates, convert them into XY coordinates using Haversine formula.
By subtracting the two XY coordinates, you’ll get direction vector. We have XY coordinates, and a direction vector. Applying atan (Recall SOH CAH TOA) gets you the angle of the track. 2 pi corrections can be done at this point.
Planes can get slightly off the track due to environmental factors or mechanical factors- that’s when cross-track error kicks in.
Cross-track error is the distance between the plane and the line that connects two waypoints.
| Code Block |
|---|
cross_track_error = cos(courseAngle) * (positionY - targetWaypointY) - sin(courseAngle) * (positionX - targetWaypointX) |
To resolve the error, we would have to apply a correction factor, desired track. If the plane is close to the line, redirecting its heading to perpendicular will cause the plane to directly cross the line, causing the same issue.
Thus, we will have to adjust the angle of its heading depending on how far the plane is from the line. The farther the plane is from the line, the angle of redirection gets closer to 90 degrees.
| Code Block | ||
|---|---|---|
| ||
desired_track = 90 - rad2deg(courseAngle - MAX_PATH_APPROACH_ANGLE * 2/PI * atan(k_gain[PATH] * pathError)) |