Updated Flight Control Algorithm

Added Accessory Functions