I see now where this is coming from. ```stabDesired.Yaw = atan2f( velocityDesired.East, velocityDesired.North ) * RAD2DEG``` depends only on VelocityDesired, which is set by updateEndpointVelocity() and updatePathVelocity(). VelocityDesired depends on path_progress, which is set by the path library.

It looks like we've exported a part of the path planning to the path follower, and a part of the path follower to the path planner. That's not good.