ardupilot/Tools/PPM_decoding/Timers.pde

125 lines
4.0 KiB
Plaintext

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++;
}