Code ready to merge with updated waypoint nav
Merged conflicts
Code for flight later today