@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.