CAREFUL: The controller is not good at controlling the pitch. I added a feature which has to be deleted probably. Look into the forces computation if interested.