zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_AVR: proper RCInput implementations for APM1 and APM2

mission-4.1.18
Pat Hickey 13 years ago committed by Andrew Tridgell
parent
fb6abbe191
commit
fc2e1455a9
3 changed files with 214 additions and 7 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      libraries/AP_HAL_AVR/RCInput.h
  2. 96
      libraries/AP_HAL_AVR/RCInput_APM1.cpp
  3. 96
      libraries/AP_HAL_AVR/RCInput_APM2.cpp

29
libraries/AP_HAL_AVR/RCInput.h
Unescape View File

@ -5,20 +5,35 @@ @@ -5,20 +5,35 @@
#include <AP_HAL.h>
#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__

96
libraries/AP_HAL_AVR/RCInput_APM1.cpp
Unescape View File

@ -0,0 +1,96 @@ @@ -0,0 +1,96 @@
#include <avr/io.h>
#include <avr/interrupt.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#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;
}

96
libraries/AP_HAL_AVR/RCInput_APM2.cpp
Unescape View File

@ -0,0 +1,96 @@ @@ -0,0 +1,96 @@
#include <avr/io.h>
#include <avr/interrupt.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#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;
}
Write Preview
Loading…
Cancel
Save