First flight

First flight on this code.
Can now readd yaw control