[rotorcraft] guidance_h: normalize psi sp from nav_heading