diff --git a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp index f1df8b2ac5..3f523ef509 100644 --- a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp +++ b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp @@ -4,60 +4,222 @@ #include #include #include "RCOutput.h" -using namespace AP_HAL; using namespace AP_HAL_AVR; -extern const HAL& hal; +extern const AP_HAL::HAL& hal; /* No init argument required */ -void APM1RCOutput::init(void* machtnicht) { +void APM1RCOutput::init(void* machtnichts) { + // --------------------- TIMER1: CH_3, CH_4, and CH_10 --------------- + hal.gpio->pinMode(11,GPIO_OUTPUT); // CH_10 (PB5/OC1A) + hal.gpio->pinMode(12,GPIO_OUTPUT); // CH_3 (PB6/OC1B) + hal.gpio->pinMode(13,GPIO_OUTPUT); // CH_4 (PB7/OC1C) + + // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1. + // CS11: prescale by 8 => 0.5us tick + TCCR1A =((1< 50hz freq + OCR1A = 0xFFFF; // Init OCR registers to nil output signal + OCR1B = 0xFFFF; + OCR1C = 0xFFFF; + + //--------------- TIMER3: CH_7, CH_8, and CH_11 --------------------- + hal.gpio->pinMode(5,GPIO_OUTPUT); // CH_11 (PE3/OC3A) + hal.gpio->pinMode(2,GPIO_OUTPUT); // CH_8 (PE4/OC3B) + hal.gpio->pinMode(3,GPIO_OUTPUT); // CH_7 (PE5/OC3C) + + // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3 + // CS31: prescale by 8 => 0.5us tick + TCCR3A =((1< 50hz freq + + //--------------- TIMER4: CH_6 and CH_5 ---------------------------- + // NB TIMER4 is shared with PPM input from RCInput_APM1.cpp + // The TIMER4 registers are assumed to be setup already. + hal.gpio->pinMode(2,GPIO_OUTPUT); // CH_5 (PE4/OC3B) + hal.gpio->pinMode(3,GPIO_OUTPUT); // CH_6 (PE5/OC3C) + + + //--------------- TIMER5: CH_1, CH_2 and CH_9 ----------------------- + hal.gpio->pinMode(46, GPIO_OUTPUT); // CH_9 (PL3/OC5A) + hal.gpio->pinMode(45, GPIO_OUTPUT); // CH_1 (PL4/OC5B) + hal.gpio->pinMode(44, GPIO_OUTPUT); // CH_2 (PL5/OC5C) + + // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR5 + // CS51: prescale by 8 => 0.5us tick + TCCR5A =((1< 50hz freq } /* Output freq (1/period) control */ void APM1RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { + uint16_t icr = _timer_period(freq_hz); + if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) { + ICR5 = icr; + } + + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) { + ICR1 = icr; + } + + if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) { + ICR3 = icr; + } + /* No change permitted for CH_5 and CH_6 - that ICR register is + * shared with the input capture for RCInput */ } uint16_t APM1RCOutput::get_freq(uint8_t ch) { - + uint16_t icr; + switch (ch) { + case CH_3: + case CH_4: + case CH_10: + icr = ICR1; + break; + /* CH_5 and CH_6 share TIMER4 with input capture. + * The period is specified in OCR4A rather than the ICR. */ + case CH_5: + case CH_6: + icr = OCR4A; + break; + case CH_7: + case CH_8: + case CH_11: + icr = ICR3; + break; + case CH_1: + case CH_2: + case CH_9: + icr = ICR5; + break; + default: + return 0; + } + /* transform to period by inverse of _time_period(icr). */ + return (2000000UL / icr); } /* Output active/highZ control, either by single channel at a time * or a mask of channels */ void APM1RCOutput::enable_ch(uint8_t ch) { - enable_mask(1 << ch); + switch(ch) { + case 0: TCCR5A |= (1<> i) & 1) { + enable_ch(i); + } + } } void APM1RCOutput::disable_ch(uint8_t ch) { - disable_mask(1 << ch); + switch(ch) { + case 0: TCCR5A &= ~(1<> i) & 1) { + disable_ch(i); + } + } +} +/* constrain pwm to be between min and max pulsewidth. */ +static inline uint16_t constrain_period(uint16_t p) { + if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH; + if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH; + return p; } /* Output, either single channel or bulk array of channels */ -void APM1RCOutput::write(uint8_t ch, uint16_t period_ms) { - +void APM1RCOutput::write(uint8_t ch, uint16_t period_us) { + /* constrain, then scale from 1us resolution (input units) + * to 0.5us (timer units) */ + uint16_t pwm = constrain_period(period_us) << 1; + switch(ch) + { + case 0: OCR5B=pwm; break; //ch1 + case 1: OCR5C=pwm; break; //ch2 + case 2: OCR1B=pwm; break; //ch3 + case 3: OCR1C=pwm; break; //ch4 + case 4: OCR4C=pwm; break; //ch5 + case 5: OCR4B=pwm; break; //ch6 + case 6: OCR3C=pwm; break; //ch7 + case 7: OCR3B=pwm; break; //ch8 + case 8: OCR5A=pwm; break; //ch9, PL3 + case 9: OCR1A=pwm; break; //ch10, PB5 + case 10: OCR3A=pwm; break; //ch11, PE3 + } } -void APM1RCOutput::write(uint8_t ch, uint16_t* period_ms, uint8_t len) { - +void APM1RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) { + for (int i = 0; i < ch; i++) { + write(i + ch, period_us[i]); + } } /* Read back current output state, as either single channel or * array of channels. */ uint16_t APM1RCOutput::read(uint8_t ch) { - + uint16_t pwm=0; + switch(ch) { + case 0: pwm=OCR5B; break; //ch1 + case 1: pwm=OCR5C; break; //ch2 + case 2: pwm=OCR1B; break; //ch3 + case 3: pwm=OCR1C; break; //ch4 + case 4: pwm=OCR4C; break; //ch5 + case 5: pwm=OCR4B; break; //ch6 + case 6: pwm=OCR3C; break; //ch7 + case 7: pwm=OCR3B; break; //ch8 + case 8: pwm=OCR5A; break; //ch9, PL3 + case 9: pwm=OCR1A; break; //ch10, PB5 + case 10: pwm=OCR3A; break; //ch11, PE3 + } + /* scale from 0.5us resolution (timer units) to 1us units */ + return pwm>>1; } -void APM1RCOutput::read(uint16_t* period_ms, uint8_t len) { - +void APM1RCOutput::read(uint16_t* period_us, uint8_t len) { + for (int i = 0; i < len; i++) { + period_us[i] = read(i); + } } uint16_t APM1RCOutput::_timer_period(uint16_t speed_hz) {