ardupilot/Tools/ArduPilotMega_demo/Timers.pde

void OutputCh(byte ch, int pwm)
{
  pwm=constrain(pwm,900,2100);
  pwm*=2;
 
 switch(ch)
  {
    case 0:  OCR5B=pwm; break;  //ch0
    case 1:  OCR5C=pwm; break;  //ch1
    case 2:  OCR1B=pwm; break;  //ch2
    case 3:  OCR1C=pwm; break;  //ch3
    case 4:  OCR4C=pwm; break;  //ch4
    case 5:  OCR4B=pwm; break;  //ch5
    case 6:  OCR3C=pwm; break;  //ch6
    case 7:  OCR3B=pwm; break;  //ch7
    case 8:  OCR5A=pwm; break;  //ch8,  PL3
    case 9:  OCR1A=pwm; break;  //ch9,  PB5
    case 10: OCR3A=pwm; break;  //ch10, PE3
  } 
  
}

int InputCh(byte ch)
{
  return (PWM_RAW[ch]+600)/2; 
}

void Init_PWM1(void)
{
  pinMode(11,OUTPUT);
  pinMode(12,OUTPUT);
  pinMode(13,OUTPUT);

  //Remember the registers not declared here remains zero by default... 
  TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all... 
  TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 2us, read page 134 of data sheet
  OCR1A = 3000; //PB5, none
  OCR1B = 3000; //PB6, OUT2
  OCR1C = 3000; //PB7  OUT3
  ICR1 = 40000; //50hz freq...Datasheet says  (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000,
}

void Init_PWM3(void)
{
  pinMode(2,OUTPUT);
  pinMode(3,OUTPUT);
  pinMode(4,OUTPUT);

  //Remember the registers not declared here remains zero by default... 
  TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all... 
  TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31); //Prescaler set to 8, that give us a resolution of 2us, read page 134 of data sheet
  OCR3A = 3000; //PE3, NONE
  OCR3B = 3000; //PE4, OUT7
  OCR3C = 3000; //PE5,  OUT6
  ICR3 = 40000; //50hz freq...Datasheet says  (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000,
}

void Init_PWM5(void)
{
  pinMode(44,OUTPUT);
  pinMode(45,OUTPUT);
  pinMode(46,OUTPUT);
  
  TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1)); 
  TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51); //Prescaler set to 8
  OCR5A = 3000; //PL3, 
  OCR5B = 3000; //PL4, OUT0
  OCR5C = 3000; //PL5  OUT1
  ICR5 = 40000;
  //ICR5 = 43910; //So (16000000hz/8)/50hz=40000,
}

/*Note that timer4 is configured to used the Input capture for PPM decoding and to pulse two servos 
  OCR4A is used as the top counter*/
void Init_PPM_PWM4(void)
{
  pinMode(49, INPUT);
  pinMode(7,OUTPUT);
  pinMode(8,OUTPUT);
      //Remember the registers not declared here remains zero by default... 
  TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));  
  TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4)); //Prescaler set to 8, that give us a resolution of 2us, read page 134 of data sheet
  OCR4A = 40000; ///50hz freq...Datasheet says  (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000, 
  //must be 50hz because is the servo standard (every 20 ms, and 1hz = 1sec) 1000ms/20ms=50hz, elementary school stuff...   
  OCR4B = 3000; //PH4, OUT5
  OCR4C = 3000; //PH5, OUT4
 
  TIMSK4 |= (1<<ICIE4); //Timer interrupt mask
  sei();
  
}
/****************************************************
  Interrupt Vector
 ****************************************************/
ISR(TIMER4_CAPT_vect)//interrupt. 
{
  if(((1<<ICES4)&TCCR4B) >= 0x01)
  { 
    if(Start_Pulse>Stop_Pulse) //Checking if the Stop Pulse overflow the register, if yes i normalize it. 
    {
      Stop_Pulse+=40000; //Nomarlizing the stop pulse.
    }
    Pulse_Width=Stop_Pulse-Start_Pulse; //Calculating pulse 
       if(Pulse_Width>5000) //Verify if this is the sync pulse
       {
        PPM_Counter=0; //If yes restart the counter
       }
       else
       {
        PWM_RAW[PPM_Counter]=Pulse_Width; //Saving pulse. 
        PPM_Counter++; 
       }
    Start_Pulse=ICR4;
    TCCR4B &=(~(1<<ICES4)); //Changing edge detector. 
  }
  else
  {
    Stop_Pulse=ICR4; //Capturing time stop of the drop edge
    TCCR4B |=(1<<ICES4); //Changing edge detector. 
    //TCCR4B &=(~(1<<ICES4));
  }
  //Counter++;
}