Added WAYPOINT_NAV mode to ControlLaw.ino