XYZ W Control implemented. A waypoint route in the NED reference frame can be followed using this version.
Added a r_wrap_pi function to limit a rotation to the range (pi:pi]