VTOL Yaw Offset in Loiter/Mission Mode in 1.7.3

Hello everyone. I am currently working off a 1.7.3 branch and I noticed in the log files a big offset between the body yaw angle and the yaw setpoint only when the VTOL vehicle is in fixed wing Loiter or Mission mode. The yaw tracks yaw setpoint very well in MC modes as well as in FW Altitude Control mode (where yaw setpoint is 0).

Here is a plot showing the yaw tracking correctly in MC mode, but notice the big offset occurring at 80seconds when it is put into a Loiter

The aircraft flies fine, though. It is doing an appropriate loiter and when we hit Return in flies the correct track back.

Here is a plot showing the transition to Return at 240s where it correct the yaw offset.