Fix: Pathing Inputs

[AskewParity]

MUST MERGE MAV UPDATES BEFORE THIS

closes #341

• airdrop pathing now starts from current plane location
• waypoint pathing starts from current plane location
  • waypoint pathing is now dynamic, with a new path generated after each lap finishes.
• adds a helper function in static to get current plane location

Testing

• run in SITL (not stress tested, just generic routes)
• passes pathing integration tests (path_planning, coverage_pathing, airdrop_approach <-- terrible naming scheme)

TODO

• Issue Dynamic Coverage Pathing #350 : coverage pathing should also by dynamic
• ticks should probably be restructured for cleaner code
• pathing performance needs to be benchmarked and optimized (rewritten lol).
• If the jetson can not make calculations quickly enough, a bandaid fix is to delete the first few waypoints in pathing under the assumption that we've already crossed them. (risk is that on a turn we have a waypoint).

[miyatakazuya]

Big thanks for fixing this. I've noticed some extraneous comments + potential bug on the path generation timeout. I didn't SITL test it yet, but should be addressed.

Also looking at the TODOs on the PR, I think the need to add a "path generation" tick is urgent. As is, if the path generation does time out and return nullptr, the isMissionFinished() may still be true because a new mission isn't uploaded yet, which would cause the lap count to decrement without the path generation succeeding.

[miyatakazuya]

I think its bad practice to modify anything from the config period(should just be const tbh). I think the current_lap should be mutable only via Mission_state

[AskewParity]

resolved