So i'm building my own quadcopter at this moment and I have designed a flight controller board for it using two atmega328p's. I've programmed it so that the first atmega receives al the data from sensors and the radio transmitter and then calculates the values for the motors, LEDs and servos. To have it run smooth, the motor controller (ESC) gets updated 250 times per second (250Hz). So the first atmega sends all the data via i2c to the second atmega 250 times per second. When the second atmega receives the data it causes an interrupt via Wire.onReceive(event);. This then sets a flag so the main loop starts when there is new data. Here is a really simplified version of my code:

atmega 1:

#include <Wire.h>
#include <CapacitiveSensor.h>

int servo1;
int val, i;

int escLR;
int escRR;
int escLF;
int escRF;

byte send_array[11];
byte error;

unsigned long int loop_timer;

void setup() {

  Wire.begin();                                                    //joins the i2c bus
  TWBR = 12;                                                      //set i2c clock to 400kHz



  loop_timer = micros();                                     //set the timer for the loop


void loop() {

  //ReadGyro();                                                              //commented out because not relevant

  if (error == 1) {
    send_array[10] &= B11111000;
    send_array[10] |= B00000001;                                   //set led bits to red

  SendData();                                                                //send the data

  if (micros() - loop_timer > 4050) {                               //set error if the loop takes too long
    error = 1;

  while (micros() - loop_timer < 4000) {}                          //wait until 4 milliseconds have passed
  loop_timer = micros();


void SendData() {                                                  //sends the data to second atmega

  send_array[1] = escLR & 0xFF;                                    //isolates the low bits
  send_array[0] = escLR >> 8;                                      //shifts high bits to the right

  send_array[3] = escRR & 0xFF;                                    //isolates the low bits
  send_array[2] = escRR >> 8;                                      //shifts high bits to the right

  send_array[5] = escLF & 0xFF;                                    //isolates the low bits
  send_array[4] = escLF >> 8;                                      //shifts high bits to the right

  send_array[7] = escRF & 0xFF;                                    //isolates the low bits
  send_array[6] = escRF >> 8;                                      //shifts high bits to the right

  send_array[9] = servo1 & 0xFF;                                   //isolates the low bits
  send_array[8] = servo1 >> 8;                                     //shifts high bits to the right

  Wire.beginTransmission(42);                                      //begins the transmission to device 42

  for (int n = 0; n < 11; n++) {

  Wire.endTransmission();                                          //sends the buffer to the slave

atmega 2:

include <ServoTimer2.h>
include <Wire.h>

int count;
int servo1_val;

int escLR_val;
int escRR_val;
int escLF_val;
int escRF_val;

byte state;
byte new_data;

byte buff[11];

unsigned long int loop_count;

ServoTimer2 escLR, escRR, escLF, escRF, servo1, servo2;           //initialize ESCs
WS2812 LED(1);                                                    //one LED
cRGB value;


void setup() {

  Wire.begin(42);                                                 //joins the i2c bus as a slave with an adress of 42
  Wire.onReceive(event);                                          //sets up the handler that gets activated when there is data coming in


  escLR.attach(8);                                                //set ESC output to pin 8
  escRR.attach(7);                                                //set ESC output to pin 7
  escLF.attach(6);                                                //set ESC output to pin 6
  escRF.attach(5);                                                //set ESC output to pin 5


void loop() {

  if (new_data == 1) {                                            //start loop if there is new data

    escLR_val = buff[0] << 8 | buff[1];                           //get the two bytes to make a 16 bit integer
    escRR_val = buff[2] << 8 | buff[3];                           //get the two bytes to make a 16 bit integer
    escLF_val = buff[4] << 8 | buff[5];                           //get the two bytes to make a 16 bit integer
    escRF_val = buff[6] << 8 | buff[7];                           //get the two bytes to make a 16 bit integer
    servo1_val = buff[8] << 8 | buff[9];                          //get the two bytes to make a 16 bit integer
    state = buff[10];                                             //get the state byte

    escLR.write(escLR_val);                                       //write to all the ESCs

    new_data = 0;


void event(int howMany) {                                         //handler that gets activated when there is data coming in
  byte count = 0;

  while (Wire.available()) {                                      //wait until all bytes are in

    buff[count] = Wire.read();

  new_data = 1;

Now here's the problem: because of the interrupt that is activated when there is data coming in, the signal for the servos and motor controller is really unstable and the servos jitter all the time. I know this because when I disable the interrupt, the servos and motors behave perfectly normal. Other forums have suggested to use the servoTimer2 library (which I do) but that didn't change a thing. Also the servos only turn 90 degrees instead of 180 but that could be because I'm running them on 250Hz instead of 50Hz. So if anyone knows how to write to the servos at 50Hz and the motor controllers at 250Hz please let me know.

So does anyone have suggestions? I'm happy with all suggestions so if I have to I will rearrange my entire code to solve this problem. I was thinking about using the using the mills() function, using requestFrom() or disabling interrupt for a while and even using hardware PWM. So maybe that brings you an idea. I'm really stuck here and I have to have it finished in less than a week so any help would be more than welcome!

by the way, english isn't my first language and am really bad at explaining things so if things are not clear I'll be happy to explain!

  • 1
    This seems like a fairly unwise design overall. If your aircraft is large enough to use ESCs rather than pager motors hanging on tiny MOSFETs, it's probably unsafe to be flying with buggy Arduino code. If your translator must use soft PWM (another bad idea), then it probably should use polled rather than interrupt based i2c receive. Commented Aug 17, 2017 at 1:37
  • 1
    At the very least, you must get rid of the while() loop in the ISR. Commented Aug 17, 2017 at 1:39
  • if I use polling, I'd have to use an interrupt on the other atmega right? So when you call the requestfrom() function the program on the first atmega must be interrupted to send the data to the second. Or is this not how polling works? I can't have an interrupt on the first atmega because there I am using another interrupt to read the PPM data.
    – nemo bondt
    Commented Aug 17, 2017 at 9:13

1 Answer 1


The core problem is that servo control on an Arduino is done using timer interrupts and not hardware PWM. And of course your I2C reception is also using interrupts.

Since the Arduino only has one level of interrupts you can't interrupt an interrupt, so when you are receiving I2C data the interrupts for the servos cannot trigger and so they get confused with bad timing signals.

You can tackle it in one of three ways (that spring to mind):

  1. Don't use interrupts for receiving your data
  2. Use hardware PWM instead of timer interrupts for the servos
  3. Use a more advanced microcontroller with multiple interrupt priority levels where the timer interrupt can interrupt the I2C interrupt.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.