I am trying to run the Madgwick filter in a CurieTimerOne interrupt service routine. It runs fine until I start using the Serial port in the Main Loop. The Timer ISR stops running after a short time. I have tracked the problem down to the CurieIMC.readMotionSensor routine. It looks like this routine running at the interrupt level has issues with the Serial port in that the Timer ISR stops running. If I move CurieIMC.readMotionSensor to the Main Loop and copy the values to the ISR it works fine. I have tried to disable interrupts for the duration of the Timer ISR but this does not solve the problem. I have attached my source file. I can not find the motion6 routine source code for the I2C I think this is where the problem is.
I have attached my source file.
clean.ino (5.83 KB)