VectorNav mostly completed, code needs some clean up, but it works