[Richard100]

Aaron -
Regarding your #15 / discontinuity in Yaw orientation at 0/360. These values are the native output of the sensor design, but will make feedback control difficult. I developed a code change that converts from the 'wrap' behavior of modulus math, to a 'wind' behavior of linear math. It detects and removes the discontinuity on the fly. It will likely work reliably at over 1000 deg/sec rotation rates (that's probably faster than most FRC bots, I think).

I've integrated it into the IMUOrientRead code so you can simply use either output, wrap or wind. I'll try to post it soon to make it easier for everyone, need to finish up some other improvements.