mirror of https://github.com/ArduPilot/ardupilot
APM_RC: PWM outputs are disabled by default on init
This commit is contained in:
parent
47204921e9
commit
a13e371d1e
|
@ -84,7 +84,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
|
||||
|
||||
//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...
|
||||
TCCR1A =((1<<WGM11)); //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 0.5us, read page 134 of data sheet
|
||||
OCR1A = 0xFFFF; // Init ODR registers to nil output signal
|
||||
OCR1B = 0xFFFF;
|
||||
|
@ -95,7 +95,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
|
||||
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
|
||||
pinMode(5,OUTPUT); //OUT10(PE3/OC3A)
|
||||
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
|
||||
TCCR3A =((1<<WGM31));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
OCR3A = 0xFFFF; // Init ODR registers to nil output signal
|
||||
OCR3B = 0xFFFF;
|
||||
|
@ -107,7 +107,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
|
||||
pinMode(46,OUTPUT); //OUT8 (PL3/OC5A)
|
||||
|
||||
TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1));
|
||||
TCCR5A =((1<<WGM51));
|
||||
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
|
||||
OCR5A = 0xFFFF; // Init ODR registers to nil output signal
|
||||
OCR5B = 0xFFFF;
|
||||
|
@ -119,7 +119,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(7,OUTPUT); //OUT5 (PH4/OC4B)
|
||||
pinMode(8,OUTPUT); //OUT4 (PH5/OC4C)
|
||||
|
||||
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
|
||||
TCCR4A =((1<<WGM40)|(1<<WGM41));
|
||||
//Prescaler set to 8, that give us a resolution of 0.5us
|
||||
// Input Capture rising edge
|
||||
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
|
||||
|
|
|
@ -80,9 +80,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(11,OUTPUT); // OUT2 (PB5/OC1A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
|
||||
// COM1A and COM1B enabled, set to low level on match.
|
||||
// CS11: prescale by 8 => 0.5us tick
|
||||
TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1));
|
||||
TCCR1A =((1<<WGM11));
|
||||
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
|
||||
ICR1 = 40000; // 0.5us tick => 50hz freq
|
||||
OCR1A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
|
@ -94,9 +93,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(6,OUTPUT); // OUT5 (PH3/OC4A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
|
||||
// COM4A, 4B, 4C enabled, set to low level on match.
|
||||
// CS41: prescale by 8 => 0.5us tick
|
||||
TCCR4A =((1<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1));
|
||||
TCCR4A =((1<<WGM41));
|
||||
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
|
||||
OCR4A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR4B = 0xFFFF;
|
||||
|
@ -109,9 +107,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
|||
pinMode(5,OUTPUT); // OUT8 (PE3/OC3A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
|
||||
// COM3A, 3B, 3C enabled, set to low level on match
|
||||
// CS31: prescale by 8 => 0.5us tick
|
||||
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
|
||||
TCCR3A =((1<<WGM31));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR3B = 0xFFFF;
|
||||
|
|
Loading…
Reference in New Issue