From 5445ad02f86d6fad26d3f204f2b1808ed900386c Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Mon, 27 Aug 2012 21:05:19 -0700 Subject: [PATCH] AP_HAL_AVR: RCOutput tested implementation for APM2 --- libraries/AP_HAL/RCOutput.h | 22 ++- libraries/AP_HAL_AVR/RCOutput.h | 12 +- libraries/AP_HAL_AVR/RCOutput_APM1.cpp | 8 +- libraries/AP_HAL_AVR/RCOutput_APM2.cpp | 173 +++++++++++++++++- .../RCPassthroughTest/RCPassthroughTest.pde | 13 +- 5 files changed, 208 insertions(+), 20 deletions(-) diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 1ec2690a70..02787780af 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -4,6 +4,22 @@ #include "AP_HAL_Namespace.h" +/* Define the CH_n names, indexed from 1, if we don't have them already */ +#ifndef CH_1 +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 +#define CH_9 8 +#define CH_10 9 +#define CH_11 10 +#endif + + class AP_HAL::RCOutput { public: virtual void init(void* implspecific) = 0; @@ -21,13 +37,13 @@ public: virtual void disable_mask(uint32_t chmask) = 0; /* Output, either single channel or bulk array of channels */ - virtual void write(uint8_t ch, uint16_t period_ms) = 0; - virtual void write(uint8_t ch, uint16_t* period_ms, uint8_t len) = 0; + virtual void write(uint8_t ch, uint16_t period_us) = 0; + virtual void write(uint8_t ch, uint16_t* period_us, uint8_t len) = 0; /* Read back current output state, as either single channel or * array of channels. */ virtual uint16_t read(uint8_t ch) = 0; - virtual void read(uint16_t* period_ms, uint8_t len) = 0; + virtual void read(uint16_t* period_us, uint8_t len) = 0; }; #endif // __AP_HAL_RC_OUTPUT_H__ diff --git a/libraries/AP_HAL_AVR/RCOutput.h b/libraries/AP_HAL_AVR/RCOutput.h index adbf71a940..3a2e4833e6 100644 --- a/libraries/AP_HAL_AVR/RCOutput.h +++ b/libraries/AP_HAL_AVR/RCOutput.h @@ -30,6 +30,9 @@ public: * array of channels. */ uint16_t read(uint8_t ch); void read(uint16_t* period_ms, uint8_t len); + +private: + uint16_t _timer_period(uint16_t speed_hz); }; class AP_HAL_AVR::APM2RCOutput : public AP_HAL::RCOutput { @@ -50,13 +53,16 @@ public: void disable_mask(uint32_t chmask); /* Output, either single channel or bulk array of channels */ - void write(uint8_t ch, uint16_t period_ms); - void write(uint8_t ch, uint16_t* period_ms, uint8_t len); + void write(uint8_t ch, uint16_t period_us); + void write(uint8_t ch, uint16_t* period_us, uint8_t len); /* Read back current output state, as either single channel or * array of channels starting at 0. */ uint16_t read(uint8_t ch); - void read(uint16_t* period_ms, uint8_t len); + void read(uint16_t* period_us, uint8_t len); + +private: + uint16_t _timer_period(uint16_t speed_hz); }; #endif // __AP_HAL_AVR_RC_OUTPUT_H__ diff --git a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp index 4888e8f6af..f1df8b2ac5 100644 --- a/libraries/AP_HAL_AVR/RCOutput_APM1.cpp +++ b/libraries/AP_HAL_AVR/RCOutput_APM1.cpp @@ -1,13 +1,16 @@ #include +#include #include #include "RCOutput.h" +using namespace AP_HAL; using namespace AP_HAL_AVR; +extern const HAL& hal; + /* No init argument required */ void APM1RCOutput::init(void* machtnicht) { - } /* Output freq (1/period) control */ @@ -57,3 +60,6 @@ void APM1RCOutput::read(uint16_t* period_ms, uint8_t len) { } +uint16_t APM1RCOutput::_timer_period(uint16_t speed_hz) { + return 2000000UL / speed_hz; +} diff --git a/libraries/AP_HAL_AVR/RCOutput_APM2.cpp b/libraries/AP_HAL_AVR/RCOutput_APM2.cpp index 33c2455a08..8aa5fe45d0 100644 --- a/libraries/AP_HAL_AVR/RCOutput_APM2.cpp +++ b/libraries/AP_HAL_AVR/RCOutput_APM2.cpp @@ -1,59 +1,212 @@ #include +#include #include #include "RCOutput.h" using namespace AP_HAL_AVR; +extern const AP_HAL::HAL& hal; + /* No init argument required */ void APM2RCOutput::init(void* machtnicht) { + // --------------------- TIMER1: OUT1 and OUT2 ----------------------- + hal.gpio->pinMode(12,GPIO_OUTPUT); // OUT1 (PB6/OC1B) + hal.gpio->pinMode(11,GPIO_OUTPUT); // OUT2 (PB5/OC1A) + // 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; + + // --------------- TIMER4: OUT3, OUT4, and OUT5 --------------------- + hal.gpio->pinMode(8,GPIO_OUTPUT); // OUT3 (PH5/OC4C) + hal.gpio->pinMode(7,GPIO_OUTPUT); // OUT4 (PH4/OC4B) + hal.gpio->pinMode(6,GPIO_OUTPUT); // OUT5 (PH3/OC4A) + + // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4. + // CS41: prescale by 8 => 0.5us tick + TCCR4A =((1< 50hz freq + + //--------------- TIMER3: OUT6, OUT7, and OUT8 ---------------------- + hal.gpio->pinMode(3,GPIO_OUTPUT); // OUT6 (PE5/OC3C) + hal.gpio->pinMode(2,GPIO_OUTPUT); // OUT7 (PE4/OC3B) + hal.gpio->pinMode(5,GPIO_OUTPUT); // OUT8 (PE3/OC3A) + + // WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3 + // CS31: prescale by 8 => 0.5us tick + TCCR3A =((1< 50hz freq + + //--------------- TIMER5: OUT10, and OUT11 --------------- + // NB TIMER5 is shared with PPM input from RCInput_APM1.cpp + // The TIMER5 registers are assumed to be setup already. + hal.gpio->pinMode(45, GPIO_OUTPUT); // OUT10 (PL4/OC5B) + hal.gpio->pinMode(44, GPIO_OUTPUT); // OUT11 (PL5/OC5C) } /* Output freq (1/period) control */ void APM2RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { + uint16_t icr = _timer_period(freq_hz); + if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) { + ICR1 = icr; + } + + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) { + ICR4 = icr; + } + + if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) { + ICR3 = icr; + } } uint16_t APM2RCOutput::get_freq(uint8_t ch) { - + uint16_t icr; + switch (ch) { + case CH_1: + case CH_2: + icr = ICR1; + break; + case CH_3: + case CH_4: + case CH_5: + icr = ICR4; + break; + case CH_6: + case CH_7: + case CH_8: + icr = ICR3; + break; + default: + return 0; + } + return (2000000UL / icr); /* inverse of _time_period(icr) */ } /* Output active/highZ control, either by single channel at a time * or a mask of channels */ void APM2RCOutput::enable_ch(uint8_t ch) { - enable_mask(1 << ch); + switch(ch) { + case 0: TCCR1A |= (1<> i; + if (c & 1) { + enable_ch(i); + } + } } void APM2RCOutput::disable_ch(uint8_t ch) { - disable_mask(1 << ch); + switch(ch) { + case 0: TCCR1A &= ~(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 APM2RCOutput::write(uint8_t ch, uint16_t period_ms) { - +void APM2RCOutput::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: OCR1B=pwm; break; // out1 + case 1: OCR1A=pwm; break; // out2 + case 2: OCR4C=pwm; break; // out3 + case 3: OCR4B=pwm; break; // out4 + case 4: OCR4A=pwm; break; // out5 + case 5: OCR3C=pwm; break; // out6 + case 6: OCR3B=pwm; break; // out7 + case 7: OCR3A=pwm; break; // out8 + case 9: OCR5B=pwm; break; // out10 + case 10: OCR5C=pwm; break; // out11 + } } -void APM2RCOutput::write(uint8_t ch, uint16_t* period_ms, uint8_t len) { - +void APM2RCOutput::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 APM2RCOutput::read(uint8_t ch) { + uint16_t pwm=0; + switch(ch) { + case 0: pwm=OCR1B; break; // out1 + case 1: pwm=OCR1A; break; // out2 + case 2: pwm=OCR4C; break; // out3 + case 3: pwm=OCR4B; break; // out4 + case 4: pwm=OCR4A; break; // out5 + case 5: pwm=OCR3C; break; // out6 + case 6: pwm=OCR3B; break; // out7 + case 7: pwm=OCR3A; break; // out8 + case 9: pwm=OCR5B; break; // out10 + case 10: pwm=OCR5C; break; // out11 + } + /* scale from 0.5us resolution (timer units) to 1us units */ + return pwm>>1; } -void APM2RCOutput::read(uint16_t* period_ms, uint8_t len) { - +void APM2RCOutput::read(uint16_t* period_us, uint8_t len) { + for (int i = 0; i < len; i++) { + period_us[i] = read(i); + } } +uint16_t APM2RCOutput::_timer_period(uint16_t speed_hz) { + return 2000000UL / speed_hz; +} diff --git a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde index 99346e7fc1..138bd3a621 100644 --- a/libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde +++ b/libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde @@ -37,7 +37,6 @@ void individualwrite(AP_HAL::RCOutput* out, uint16_t* channels) { for (int ch = 0; ch < 8; ch++) { out->write(ch, channels[ch]); } - } void loop (void) { @@ -46,7 +45,6 @@ void loop (void) { hal.gpio->write(27, 1); - /* Cycle between using the individual read method * and the multi read method*/ if (ctr < 500) { @@ -70,9 +68,18 @@ void loop (void) { void setup (void) { hal.uart0->begin(115200); - hal.uart0->printf_P(PSTR("reading rc in:")); hal.gpio->pinMode(27, GPIO_OUTPUT); hal.gpio->write(27, 0); + hal.rcout->enable_mask(0x000000FF); + + /* Bottom 4 channels at 400hz (like on a quad) */ + hal.rcout->set_freq(0x0000000F, 400); + for(int i = 0; i < 12; i++) { + hal.uart0->printf_P(PSTR("rcout ch %d has frequency %d\r\n"), + i, hal.rcout->get_freq(i)); + } + /* Delay to let the user see the above printouts on the terminal */ + hal.scheduler->delay(1000); }