Control de múltiples ESC con ATMEGA128 PWM

Así que he estado trabajando en este Quadcopter por un tiempo, finalmente lo construí todo y lo armé. Intentando escribir algún código para controlar el ESC (RedBrick HobbyKing 30A con UBEC). Logré armar un ESC de acuerdo con la documentación y programarlo usando 50Hz Fast PWM en un ATMEGA128, controlando un motor.

El problema que tengo ahora es que cuando trato de usar múltiples motores, tengo problemas, todos parecen armarse correctamente, es solo cuando trato de darle energía, está fallando. Cualquiera de los motores comenzará a girar mientras el otro sigue emitiendo un pitido o viceversa. A veces ninguno se ejecutará. Intenté solucionar este problema usando una interrupción que se disparará cuando se alcance el ICR1 (TCCR1A). Echa un vistazo al código para más detalles.

PD. Me gustaría entender la lógica detrás de este problema, por qué ocurre y cuáles son algunas soluciones comunes. Tenga en cuenta que soy nuevo en la programación de AVR, por lo que se prefiere la terminología genérica. ¡Gracias!

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define ESC_LOW             1000    //low duty cycle in milliseconds
#define ESC_HIGH            2000    //high duty cycle in milliseconds
#define motors_ddr          DDRB    //the data direction register the motors will operate on
#define motors_port         PORTB   //the port the motors will operate on
#define north_motor         4       //pin number for this motor
#define south_motor         5       //pin number for this motor
#define east_motor          6       //pin number for this motor
#define west_motor          7       //pin number for this motor


void init_ESCs(void) {

    //set all motors pins to output
    //motors_ddr |= 1<<north_motor | 1<<south_motor | 1<<east_motor | 1<<west_motor;
    motors_ddr = 0xFF;
    /*
        Set the Waveform Generation Mode to Fast PWM
        Set WGM11, WGM12 & WGM13 to select Fast PWM
        Prescaler: 8 (set the CS11 bit)
        Set OCIE1A bit in TIMSK to enable the Output Compare Interrupt Enabled Register
    */
    TCCR1A |= 1<<WGM11;
    TCCR1B |= 1<<WGM13 | 1<<WGM12 | 1<<CS11;
    TIMSK |= 1<<OCIE1A;

    //my CPU is at 16MHz
    //Set the TOP i.e. Top = [ cpu_clk_speed Hz / (Prescaler) * (Frequency Hz) ] - 1
    ICR1 = 39999;

    //how many times to repeat the signal
    volatile int armCount = 170;
    volatile int confirmCount = 150;

    //this loop sends the 2ms signal
    while(armCount > 0) {

            if(TCNT1 >= ESC_HIGH && bit_is_set(motors_port, north_motor) && bit_is_set(motors_port, south_motor) && bit_is_set(motors_port, east_motor) && bit_is_set(motors_port, west_motor)) {
                armCount--;
                motors_port &= ~(1<<north_motor | 1<<south_motor | 1<<east_motor | 1<<west_motor);
                //motors_port &= 0b11110000; //set motor PINS LOW
            }
    }

    //wait a while before confirming the Arming process (dunno if this is necessary)
    _delay_ms(500);

    //this loop sends the 1ms signal completing the arming
    while(confirmCount > 0) {
        if(TCNT1 >= ESC_LOW && bit_is_set(motors_port, north_motor) && bit_is_set(motors_port, south_motor) && bit_is_set(motors_port, east_motor) && bit_is_set(motors_port, west_motor)) {

            confirmCount--;
            //motors_port &= 0b11110000; //set motor PINS LOW
            motors_port &= ~(1<<north_motor | 1<<south_motor | 1<<east_motor | 1<<west_motor);
        }
    }
}


void speed_test(int speed) {
    TCCR1A |= 1<<WGM11;
    TCCR1B |= 1<<WGM13 | 1<<WGM12 | 1<<CS11;
    TIMSK |= 1<<OCIE1A;

    ICR1 = 39999;

    while(1) {
        if(TCNT1 >= speed && bit_is_set(motors_port, north_motor) && bit_is_set(motors_port, south_motor) && bit_is_set(motors_port, east_motor) && bit_is_set(motors_port, west_motor)) {

            //motors_port &= 0b11110000; //set motor PINS LOW
            motors_port &= ~(1<<north_motor | 1<<south_motor | 1<<east_motor | 1<<west_motor);
        }
    }   
}

int main(void) {

    //set enabled interrupt bit
    sei();

    init_ESCs();

    //speed_test(1500); //1.5ms signal

    while(1) {

    }   
}

ISR (TIMER1_COMPA_vect) {
    //set all motor pins to HIGH
    motors_port |= 1<<north_motor | 1<<south_motor | 1<<east_motor | 1<<west_motor; 
}