Initial working version running as a mavproxy module.  Needs a lot of clean up.