Thanks @Tropical and @Mentor_355U!
Yeah, so the datarate did turn out to be 10ms after all that. I guess I should have reread the post again: Rotation sensor update rate? (Optical shaft encoder vs rotation sensor?) - Programming Support - VEX Forum. But at least better than the 20ms which is where I started.
Disconnects are interesting - will need to pay attention to that.
The changed callbacks didn’t quite work out after all that. Turns out that by the time the main odometry loop starts the saved sensor readings were quite often stale. Not sure if this was because of filtering of values in the API or scheduling.
So the best I seem to be able to do here is to still run the sensor sampling loop every 5ms to try to get the updated value as close as reasonable without taking up too much CPU time. These values then get placed in a small buffer that keeps track of the last few readings along with their timestamps. The timestamps are the best proxy I can find for that actual time of the sensor reading.
The main odometry loop then runs at 10ms and uses a filter (linear interpolation for now) to retime/resample the readings. Results for a robot driving straight for 1m below. This is only the heading part of the calculation.
First Chart: This is where I started with 20ms datarate and no interpolation. You can see the discontinuities where the sensors switch relative places in time with one another. The gyro is included here as a reference. All values in degrees.
Second Chart: 10ms samples with linear interpolation for retiming. Still a bit noisy, but much improved. Sorry no gyro here.
Anyway, onto figuring out which filter to use …