Hefty Timer3 Interrupt is messing with PWM signals to Servos

I’m using an Arduino Mega 2560 and 3 servos (pins 9, 10, 11). I am using timer3 and an output compare match interrupt to create an interrupt at a given frequency (200Hz). To get this timer to work, I had to go into ServoTimers.h and comment out the “#define Timer3” line and it usage because I was running into a conflict.

According to Arduino 101: Timers and Interrupts - Tutorials - RobotShop Community,

On the Arduino Mega we have 6 timers and 15 PWM outputs:
Pins 4 and 13: controlled by timer0
Pins 11 and 12: controlled by timer1
Pins 9 and10: controlled by timer2
Pin 2, 3 and 5: controlled by timer 3
Pin 6, 7 and 8: controlled by timer 4
Pin 46, 45 and 44:: controlled by timer 5

My goal was to have the timer interrupt be at a constant frequency so I could read some signals, do A LOT of math ( getFourier(), which takes ~2ms), and then control my servos appropriately. I wanted to put all this in the interrupt, but I know that is not how you are supposed to use interrupts, so instead I just set a bool flag and then in the loop, I check for the flag and do all the math. Because I am dependent on the loop, I am not guaranteed to be doing this math at regular time intervals, and due to everything else in the loop, it occurs at ~130Hz to 160Hz. This is a problem for my frequency analysis in getFourier().

When I try putting all the math into the interrupt, the Servos start acting up. but the math occurs at regular time intervals (I verified the frequency of the interrupts). I find this odd because the servos should be controlled by timer5 (see ServoTimers.h below). If I decrease the sample frequency (aka. the frequency at which this interrupt is triggered) then my servos act more normally.

My best guess for why this is happening is that the hefty interrupt is also interrupting the Servo pwm signal for a significant period of time, even though they are rooted in different timers. Is this true? If not, is there a way I can get continuous servo control as well as hit a hefty interrupt at a high frequency?

Setup for Servos and Timer3 Interrupt

void setup()
{
  analogReference(EXTERNAL);//3.3V from the teensy is connected to the AREF pin on the Arduino so the arduino knows that the signal goes between 0-3.3V


  /*
      initialize timer4, timer4's definition and usage has been commented out in ServoTimers.h for the AVR_ATmega2560 chip
      "C:\Program Files (x86)\Arduino\libraries\Servo\src\avr\ServoTimers.h"
  */
  noInterrupts();           // disable all interrupts
  TCCR3A = 0;
  TCCR3B = 0;
  TCNT3  = 0;
  OCR3A = floor(16000000 / 256 / sampleRate_Hz);        // compare match register 16MHzclock speed/256 prescale/~200Hz sample rate (200Hz is user specified)
  TCCR3B |= (1 << WGM32);   // CTC mode
  TCCR3B |= (1 << CS32);    // 256 prescaler
  TIMSK3 |= (1 << OCIE3A);  // enable timer compare interrupt
  interrupts();             // enable all interrupts

Serial.begin(9600);
  ESC.attach(8);
  servo1.attach(9);   // attaches the servo on pin 9 to the servo object
  servo2.attach(10);  // attaches the servo on pin 10 to the servo object
  servo3.attach(11);  // attaches the servo on pin 11 to the servo object

  // move servos to initial position
  setServos(positions[0], positions[1], positions[2]);

......
}

Modified ServoTimers.h

// Say which 16 bit timers can be used and in what order
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define _useTimer5
#define _useTimer1
//#define _useTimer3
#define _useTimer4
typedef enum { _timer5, _timer1,/* _timer3,*/ _timer4, _Nbr_16timers } timer16_Sequence_t;

Timer Interrupt Definition (working definition followed by definition that causes servos to act up):

ISR(TIMER3_COMPA_vect)          // timer compare interrupt service routine
{
  sampleInterrupt = true; //this bool will cause getFourier() to be called in the loop()
}

ISR(TIMER3_COMPA_vect)          // timer compare interrupt service routine
{
  getFourer(); //this function takes ~2ms to run and adding it to the interrupt causes the PWM signals to be erratic

}

Conflict when using Timer1 or any timer that was defined in ServoTimers.h (reason why I had to comment out timer3):

Arduino: 1.8.7 (Windows 7), TD: 1.44, Board: "Arduino/Genuino Mega or Mega 2560, ATmega2560 (Mega 2560)"

libraries\Servo\avr\Servo.cpp.o (symbol from plugin): In function `ServoCount':

(.text+0x0): multiple definition of `__vector_17'

sketch\Servo_ESC_CommandLine_20190802_readLoads.ino.cpp.o (symbol from plugin):(.text+0x0): first defined here

c:/program files (x86)/arduino/hardware/tools/avr/bin/../lib/gcc/avr/5.4.0/../../../../avr/bin/ld.exe: Disabling relaxation: it will not work with multiple definitions

collect2.exe: error: ld returned 1 exit status

exit status 1
Error compiling for board Arduino/Genuino Mega or Mega 2560.

This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.

Why are your running your “2ms worth of math” in the ISR itself?

You got the timing working, set a flag in the ISR, get out and run the math in your main code if the flag is set.

Sure maybe the “math” might take 2.02ms or some other incredibly low extra amount of time now, but everything else will run properly.

I want getFourier() to occur at regularly spaced 200Hz intervals. There is a lot of other stuff in the loop() so when I put the math there, it occurs at a speed of about 140Hz.

If interrupts are regularly disabled for more than a microsecond (16 instruction cycles) the servo interrupts will be delayed and that will throw the timing off. The longer the delay the worse the jitter. Re-enable interrupts in your huge ISR, after you gather the data that has to be at fixed intervals. That way the servo interrupt won't be badly delayed.

Thanks!