Added Flight Controller code from Arducopter repo