Since this should only run for attitude mode the yaw desired actually does need to be modulus'd to wrap around properly.  However, do we even want to apply this to yaw?  Is there ever a need to make yaw attitude mode have an offset from the body frame? 

Remember yaw attitude mode means your stick selects a compass heading.