Updated home position handling, should get waypoints right now