Calls to setUpdateRateHz, zeroyaw, zeroDisplacement freeze robot code for at least 1ms, but up to 16ms

[virtuald]

This is a similar issue to one that affected NetworkTables in prior years. When calling any of those APIs, they eventually call into the RegisterIO_* functions... which try to obtain the same lock that is being used by the I/O read thread. The I/O read thread may potentially block for at least 0.001s, if not longer depending on the update rate -- a read could block for up to 16ms at a 60hz update rate. This seems quite bad.

Reference: https://github.com/kauailabs/navxmxp/blob/master/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_SPI.java#L74

I would propose either having a separate write thread, or perhaps create some queue that commands could be pushed onto.

[virtuald]

Also, it's a bit strange anyways, why is that wait there? Seems like you would want to use spi.transaction, right?

[kauailabs]

The navX-MXP processor is very busy (in addition to the 4 communication interfaces it servcies, it's also got several cases where it can't respond immediately. The wait is there to handle the worst case response time between when the initial request is sent on the SPI bus and the navX-MXP firwmare is ready to actually respond with a response.