@marwijn I forget the max safe time, but I thought it was around 12us max inside a "no interrupts" sandwich. Do you sepecifically stop interrupts inside your ISR?
Generally, I think your best approach is leave it in the loop and code without using
delay and use
micros to time the loop update to apply to calculations rather than code for a constant 5ms.