I fully agree that the best is have everyting in μs, so also in the wheel encoders signals interrupt handler (and the SITL simulation). Anyhow, I don’t know the code but I downloaded your code from GitHub and compiled it getting:
Target Text (B) Data (B) BSS (B) Total Flash Used (B) Free Flash (B)
--------------------------------------------------------------------------------
bin/ardurover 1395052 2116 194708 1397168 683584
for a Pixhawk1.
I tested it on a mission indoors (rover balance bot, no GPS, using wheel encoders and the Pixhawk1 internal compass):
As in here, it completes a lap and a bit more. Obviously wheel encoders positioning doesn’t last forever.
To test on your code the monolythic irq handler with code simplifications, no call/returns and no division μs->ms, I substituted the five files above, resulting:
Target Text (B) Data (B) BSS (B) Total Flash Used (B) Free Flash (B)
--------------------------------------------------------------------------------
bin/ardurover 1395016 2116 194704 1397132 683624
(40 bytes less, even with the remainder computations)
and tested the same mission:
with almost identical results.
BTW1, note that function AP_WheelEncoder_Quadrature::pin_ab_to_phase() in WheelEncoder_Quadrature.cpp contains two code alternatives and the not executed one is incorrect. The correct code can be put in a single line within the irq handler as:
uint8_t phase_after = (last_pin_a_value?(last_pin_b_value?2:3):(last_pin_b_value?1:0));
BTW2, tuning could be improved if I knew how (compromise tilt oscillation/motors teeth knocking, possibly worse at low speeds in above videos).
BTW3, the lap is quite difficult with reduced space, having to pass four doors (D1, D2, D3, D4) and passing beside a refrigerator R which affects the Pixhawk1 internal compass:
