Small changes
The controller is finally working in Pitch Roll and Yaw.