Flight code from 2nd flight on 5/24
Merged code flown on 5/24 with new waypoint nav code in medium loop