I'd appreciate to see your controller in a quadrotor roll + pitch control. Basically, as simple and preliminary evaluation, you can simplify the problem for the 2-DoF problem and decouple roll and pitch non-linear equations. I have tested several controllers on a bench like that. You may resort to the model in the following link: