Implement or use PIDs for yaw, pitch, roll control

[AVSurfer123]

Look at uuv_control_cascaded_pids as an example. If it can be used, use the same package for controlling absolute orientation.