[rotorcraft][stabilization] add quat_from_rpy_cmd function

Mostly only refactoring, but also use the quat_from_rpy_cmd when converting the "euler" cmd reference to quaternion.
This is still not working as expected :-(