mirror of https://github.com/ArduPilot/ardupilot
125 lines
4.0 KiB
Plaintext
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++;
|
|
}
|