I have similiar situation in new 3.5-rc6 firmware.
I uploaded the problem on the github with logs.
but couldn't find someone experienced same problem until now.
I have real frame having issue with heading controller and simulations also.
which is heading controller keep rotates vehicle.
regardless of flight modes, (I have tested auto, poshold, and althold mode)
looks like heading rate command diverges for some time.
I suspect new function here, but I couldn't understand this function yet.
Problem video with SITL + Gazebo is here.