Added control from GCS, including Arming/disarming, also cleaned up some code