From fc2e1455a92243e6b527f6e83d9ab7e509060293 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Mon, 27 Aug 2012 16:45:27 -0700 Subject: [PATCH] AP_HAL_AVR: proper RCInput implementations for APM1 and APM2 --- libraries/AP_HAL_AVR/RCInput.h | 29 ++++++-- libraries/AP_HAL_AVR/RCInput_APM1.cpp | 96 +++++++++++++++++++++++++++ libraries/AP_HAL_AVR/RCInput_APM2.cpp | 96 +++++++++++++++++++++++++++ 3 files changed, 214 insertions(+), 7 deletions(-) create mode 100644 libraries/AP_HAL_AVR/RCInput_APM1.cpp create mode 100644 libraries/AP_HAL_AVR/RCInput_APM2.cpp diff --git a/libraries/AP_HAL_AVR/RCInput.h b/libraries/AP_HAL_AVR/RCInput.h index 3be412dfa8..7062aabfe7 100644 --- a/libraries/AP_HAL_AVR/RCInput.h +++ b/libraries/AP_HAL_AVR/RCInput.h @@ -5,20 +5,35 @@ #include #include "AP_HAL_AVR_Namespace.h" +#define AVR_RC_INPUT_NUM_CHANNELS 8 + class AP_HAL_AVR::APM1RCInput : public AP_HAL::RCInput { public: - APM1RCInput() : _init(0) {} - void init(int machtnicht) { _init = 1; } + /* Pass in a AP_HAL_AVR::ISRRegistry* as void*. */ + void init(void* isrregistry); + uint8_t valid(); + uint16_t read(uint8_t ch); + uint8_t read(uint16_t* periods, uint8_t len); private: - int _init; + /* private callback for input capture ISR */ + static void _timer4_capt_cb(void); + /* private variables to communicate with input capture isr */ + static volatile uint16_t _pulse_capt[AVR_RC_INPUT_NUM_CHANNELS]; + static volatile uint8_t _valid; }; class AP_HAL_AVR::APM2RCInput : public AP_HAL::RCInput { -public: - APM2RCInput() : _init(0) {} - void init(int machtnicht) { _init = 2; } + /* Pass in a AP_HAL_AVR::ISRRegistry* as void*. */ + void init(void* isrregistry); + uint8_t valid(); + uint16_t read(uint8_t ch); + uint8_t read(uint16_t* periods, uint8_t len); private: - int _init; + /* private callback for input capture ISR */ + static void _timer5_capt_cb(void); + /* private variables to communicate with input capture isr */ + static volatile uint16_t _pulse_capt[AVR_RC_INPUT_NUM_CHANNELS]; + static volatile uint8_t _valid; }; #endif // __AP_HAL_AVR_RC_INPUT_H__ diff --git a/libraries/AP_HAL_AVR/RCInput_APM1.cpp b/libraries/AP_HAL_AVR/RCInput_APM1.cpp new file mode 100644 index 0000000000..274330702b --- /dev/null +++ b/libraries/AP_HAL_AVR/RCInput_APM1.cpp @@ -0,0 +1,96 @@ + +#include +#include + +#include +#include +#include "RCInput.h" +#include "utility/ISRRegistry.h" +using namespace AP_HAL; +using namespace AP_HAL_AVR; + +extern const HAL& hal; + +/* private variables to communicate with input capture isr */ +volatile uint16_t APM1RCInput::_pulse_capt[AVR_RC_INPUT_NUM_CHANNELS] = {0}; +volatile uint8_t APM1RCInput::_valid = 0; + +/* private callback for input capture ISR */ +void APM1RCInput::_timer4_capt_cb(void) { + static uint16_t icr4_prev; + static uint8_t channel_ctr; + + const uint16_t icr4_current = ICR4; + uint16_t pulse_width; + if (icr4_current < icr4_prev) { + /* ICR4 rolls over at TOP=40000 */ + pulse_width = icr4_current + 40000 - icr4_prev; + } else { + pulse_width = icr4_current - icr4_prev; + } + + if (pulse_width > 8000) { + /* sync pulse detected */ + channel_ctr = 0; + } else { + if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) { + _pulse_capt[channel_ctr] = pulse_width; + channel_ctr++; + } else { + _valid = AVR_RC_INPUT_NUM_CHANNELS; + } + } + icr4_prev = icr4_current; +} + +void APM1RCInput::init(void* _isrregistry) { + ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry; + isrregistry->register_signal(ISR_REGISTRY_TIMER4_CAPT, _timer4_capt_cb); + + /* Arduino pin 49 is ICP4 / PL0, timer 4 input capture */ + hal.gpio->pinMode(49, GPIO_INPUT); + /** + * WGM: 1 1 1 1. Fast WPM, TOP is in OCR4A + * COM all disabled + * CS41: prescale by 8 => 0.5us tick + * ICES4: input capture on rising edge + * OCR4A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound + * fast PWM. + */ + TCCR4A = _BV(WGM40) | _BV(WGM41); + TCCR4B = _BV(WGM43) | _BV(WGM42) | _BV(CS41) | _BV(ICES4); + OCR4A = 40000; + + /* OCR4B and OCR4C will be used by RCOutput_APM1. init to nil output */ + OCR4B = 0xFFFF; + OCR4C = 0xFFFF; + + /* Enable input capture interrupt */ + TIMSK4 |= _BV(ICIE4); +} + +uint8_t APM1RCInput::valid() { return _valid; } + +#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +uint16_t APM1RCInput::read(uint8_t ch) { + cli(); + uint16_t capt = _pulse_capt[ch]; + sei(); + uint16_t capt_usec = capt >> 1; + _valid = 0; + return constrain(capt_usec, + RC_INPUT_MIN_PULSEWIDTH, RC_INPUT_MAX_PULSEWIDTH); +} + +uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) { + cli(); + for (int i = 0; i < len; i++) { + periods[i] = _pulse_capt[i] >> 1; + } + sei(); + uint8_t v = _valid; + _valid = 0; + return v; +} + diff --git a/libraries/AP_HAL_AVR/RCInput_APM2.cpp b/libraries/AP_HAL_AVR/RCInput_APM2.cpp new file mode 100644 index 0000000000..07c9624ec1 --- /dev/null +++ b/libraries/AP_HAL_AVR/RCInput_APM2.cpp @@ -0,0 +1,96 @@ + +#include +#include + +#include +#include +#include "RCInput.h" +#include "utility/ISRRegistry.h" +using namespace AP_HAL; +using namespace AP_HAL_AVR; + +extern const HAL& hal; + +/* private variables to communicate with input capture isr */ +volatile uint16_t APM2RCInput::_pulse_capt[AVR_RC_INPUT_NUM_CHANNELS] = {0}; +volatile uint8_t APM2RCInput::_valid = 0; + +/* private callback for input capture ISR */ +void APM2RCInput::_timer5_capt_cb(void) { + static uint16_t icr5_prev; + static uint8_t channel_ctr; + + const uint16_t icr5_current = ICR5; + uint16_t pulse_width; + if (icr5_current < icr5_prev) { + /* ICR5 rolls over at TOP=40000 */ + pulse_width = icr5_current + 40000 - icr5_prev; + } else { + pulse_width = icr5_current - icr5_prev; + } + + if (pulse_width > 8000) { + /* sync pulse detected */ + channel_ctr = 0; + } else { + if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) { + _pulse_capt[channel_ctr] = pulse_width; + channel_ctr++; + } else { + _valid = AVR_RC_INPUT_NUM_CHANNELS; + } + } + icr5_prev = icr5_current; +} + +void APM2RCInput::init(void* _isrregistry) { + ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry; + isrregistry->register_signal(ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb); + + /* Arduino pin 48 is ICP5 / PL1, timer 5 input capture */ + hal.gpio->pinMode(48, GPIO_INPUT); + /** + * WGM: 1 1 1 1. Fast WPM, TOP is in OCR5A + * COM all disabled + * CS51: prescale by 8 => 0.5us tick + * ICES5: input capture on rising edge + * OCR5A: 40000, 0.5us tick => 2ms period / 50hz freq for outbound + * fast PWM. + */ + TCCR5A = _BV(WGM50) | _BV(WGM51); + TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS51) | _BV(ICES5); + OCR5A = 40000; + + /* OCR5B and OCR5C will be used by RCOutput_APM2. init to nil output */ + OCR5B = 0xFFFF; + OCR5C = 0xFFFF; + + /* Enable input capture interrupt */ + TIMSK5 |= _BV(ICIE5); +} + +uint8_t APM2RCInput::valid() { return _valid; } + +#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) + +uint16_t APM2RCInput::read(uint8_t ch) { + cli(); + uint16_t capt = _pulse_capt[ch]; + sei(); + uint16_t capt_usec = capt >> 1; + _valid = 0; + return constrain(capt_usec, + RC_INPUT_MIN_PULSEWIDTH, RC_INPUT_MAX_PULSEWIDTH); +} + +uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) { + cli(); + for (int i = 0; i < len; i++) { + periods[i] = _pulse_capt[i] >> 1; + } + sei(); + uint8_t v = _valid; + _valid = 0; + return v; +} +