Problem running Madgwich in ISR

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)

One more piece of information. When I pulled the CurieIMC.readMotionSensor routine out of the timer interrupt I was still getting issues with the serial port. I was getting occasional NAN (not a number) being reported by the program reading the serial data independent of UART speed. When I removed the entire timer ISR I was able to run the serial port at 115200 and run the Madgwick filter at 3200Hz in the main loop.

best_115200.ino (5.5 KB)