Yaw reference somehow goes up to pi or -pi when the output of the dr16 stick is capped from -pi/2 to pi/2

[Rishi-H]

Most likely an issue with the reference governor.

[Sammyboyo]

This was determined to be because of long loop times, very likely caused by the logging prints to serial.