Calculates current heading

[jamenkaye]

Description

• Resolves #283
• Migrated from local_pathfinding; original issue: https://github.com/UBCSailbot/local_pathfinding/pull/88

TODO:

• [ ] Store the local waypoint we're currently trying to reach
• [ ] Update the local waypoint we're trying to reach when we reach it

Verification

• [ ]

Resources

[Krithik1]

1. Add a field in self to store the waypoint we are going towards
2. Original Desired Heading (field in Sailbot class)

What get_desired_heading has to do?

1. Update the current waypoint that we are trying to reach (separate function)
2. Now if in both problem zones then publish old heading
3. If not in the problem zones then calculate heading from curr position to current waypoint

[jamenkaye]

Just adding some of the other details we talked about today:

How do we update the target waypoint that we're trying to reach?

• If we're REALLY close (within like 100m or something) of the target waypoint (small circle region), count the waypoint as successfully reached
  • Update the target waypoint
  • update the "original heading" - the angle from the current boat position to the waypoint
• If we're in the problem zone of the target waypoint, and NOT in the problem zone of the successive waypoint:
  • Update the target waypoint
  • update the "original heading"
    • If we're in the problem zone of BOTH the target and successive waypoints, then we'll keep sailing on the "old heading" so we don't need to update anything
    • If we aren't in either problem zone, don't need to update anything.

I'm saying "target waypoint" to mean the waypoint that we are currently trying to navigate towards, and "successive waypoint" to mean the next waypoint after the target waypoint.

How do we decide if we're in the problem zone of some given waypoint? It depends on:

• The current GPS position of the boat
• The position of the waypoint
• the angle of the wind
• The threshold angle that is considered to be the problem zone - we may want this threshold to be different depending if we're considering the target waypoint or the successive waypoint

Then I think you'd basically analyze (the true wind angle) - (the bearing of the sailboat from the waypoint). I would definitely suggest drawing this out, to make sure you account for the problem zone that extends downwind as well as upwind of the waypoint (the whole green-lantern shape)

Lmk if you have any more questions!! Happy coding! ⌨🖥👨‍💻📐