Considering the corrected line of code in …/ArduPilot/ardupilot/blob/Rover-4.2.1/libraries/AP_WheelEncoder/WheelEncoder_Quadrature.cpp:
irq_state.last_reading_ms = timestamp * 1e-3f;
for an encoder with 500 lines which rotates at 50 rps it is going to add 25000 floating point operations per second (or 50000 if at both edges) per wheel and per signal (two in quadrature), and there at least two or may be four wheels with encoders, and much worse if within interrupts.
Also, I am not sure if member irq_state.last_reading_ms is used in any calculation or is only informative. If so, above is a full waste of time.
So I think the best would be to use μs:
- rename the struct member to irq_state.last_reading_us;
- leave above line as irq_state.last_reading_us = timestamp; (or comment it and see if there is any anomaly).
On a test similar to above with the new code, but travelling the four mission waypoints alternatively CW and CCW this happened:
The difference between traces is significant, all data for both wheels are the same, and I can’t find any diameter difference. I will exchange both wheels and see what happens, but I wonder if this anomaly is related to the additional floating point operation.