mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_AVR: mostly working implementation of RCOutput_APM1.cpp
* CH5 and CH6 output doesn't work at the moment. not sure why
This commit is contained in:
Pat Hickey
Andrew Tridgell
parent
d8ceb427f4
commit
875e928628
|
|
@ -4,60 +4,222 @@
|
|||
#include <AP_HAL.h>
|
||||
#include <AP_HAL_AVR.h>
|
||||
#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<<WGM11));
|
||||
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
|
||||
ICR1 = 40000; // 0.5us tick => 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<<WGM31));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR3B = 0xFFFF;
|
||||
OCR3C = 0xFFFF;
|
||||
ICR3 = 40000; // 0.5us tick => 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<<WGM51));
|
||||
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
|
||||
OCR5A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR5B = 0xFFFF;
|
||||
OCR5C = 0xFFFF;
|
||||
ICR5 = 40000; // 0.5us tick => 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<<COM5B1); break; // CH_1 : OC5B
|
||||
case 1: TCCR5A |= (1<<COM5C1); break; // CH_2 : OC5C
|
||||
case 2: TCCR1A |= (1<<COM1B1); break; // CH_3 : OC1B
|
||||
case 3: TCCR1A |= (1<<COM1C1); break; // CH_4 : OC1C
|
||||
case 4: TCCR4A |= (1<<COM4C1); break; // CH_5 : OC4C
|
||||
case 5: TCCR4A |= (1<<COM4B1); break; // CH_6 : OC4B
|
||||
case 6: TCCR3A |= (1<<COM3C1); break; // CH_7 : OC3C
|
||||
case 7: TCCR3A |= (1<<COM3B1); break; // CH_8 : OC3B
|
||||
case 8: TCCR5A |= (1<<COM5A1); break; // CH_9 : OC5A
|
||||
case 9: TCCR1A |= (1<<COM1A1); break; // CH_10: OC1A
|
||||
case 10: TCCR3A |= (1<<COM3A1); break; // CH_11: OC3A
|
||||
}
|
||||
}
|
||||
|
||||
void APM1RCOutput::enable_mask(uint32_t chmask) {
|
||||
|
||||
for (int i = 0; i < 32; i++) {
|
||||
if ((chmask >> i) & 1) {
|
||||
enable_ch(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void APM1RCOutput::disable_ch(uint8_t ch) {
|
||||
disable_mask(1 << ch);
|
||||
switch(ch) {
|
||||
case 0: TCCR5A &= ~(1<<COM5B1); break; // CH_1 : OC5B
|
||||
case 1: TCCR5A &= ~(1<<COM5C1); break; // CH_2 : OC5C
|
||||
case 2: TCCR1A &= ~(1<<COM1B1); break; // CH_3 : OC1B
|
||||
case 3: TCCR1A &= ~(1<<COM1C1); break; // CH_4 : OC1C
|
||||
case 4: TCCR4A &= ~(1<<COM4C1); break; // CH_5 : OC4C
|
||||
case 5: TCCR4A &= ~(1<<COM4B1); break; // CH_6 : OC4B
|
||||
case 6: TCCR3A &= ~(1<<COM3C1); break; // CH_7 : OC3C
|
||||
case 7: TCCR3A &= ~(1<<COM3B1); break; // CH_8 : OC3B
|
||||
case 8: TCCR5A &= ~(1<<COM5A1); break; // CH_9 : OC5A
|
||||
case 9: TCCR1A &= ~(1<<COM1A1); break; // CH_10: OC1A
|
||||
case 10: TCCR3A &= ~(1<<COM3A1); break; // CH_11: OC3A
|
||||
}
|
||||
}
|
||||
|
||||
void APM1RCOutput::disable_mask(uint32_t chmask) {
|
||||
for (int i = 0; i < 32; i++) {
|
||||
if ((chmask >> 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) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue