Refactored code, added copter CLA controller, and resetting every N iterations