Updated Flight Control Algorithm

Fixed broken mallocs