--Traced slowdown of >100ms to write_gps() function, which only calls gps_read_data().  Starting to look into that function to see what is taking so long to return.
--Also saw that write_imu_encoders() is consistently taking 30-45ms to run.  Once I have the GPS problem fixed I need to look into that, although that delay will still allow us to run the sensors at 20Hz, which may be OK.
--Started working on my own GPS reader/parser to keep up with loop speed--fgets and readLine run too slow.