Implemented more full controller for copter, and refactored code