Updated flightControl and added first fly test