Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_AVR: proper RCInput implementations for APM1 and APM2

Browse Source
This commit is contained in:
Pat Hickey 2012-08-27 16:45:27 -07:00 committed by Andrew Tridgell
parent fb6abbe191
commit fc2e1455a9
3 changed files with 214 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_HAL_AVR/RCInput.h
View File

@ -5,20 +5,35 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include "AP_HAL_AVR_Namespace.h" #include "AP_HAL_AVR_Namespace.h"
#define AVR_RC_INPUT_NUM_CHANNELS 8
class AP_HAL_AVR::APM1RCInput : public AP_HAL::RCInput { class AP_HAL_AVR::APM1RCInput : public AP_HAL::RCInput {
public: public:
APM1RCInput() : _init(0) {} /* Pass in a AP_HAL_AVR::ISRRegistry* as void*. */
void init(int machtnicht) { _init = 1; } void init(void* isrregistry);
uint8_t valid();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
private: 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 { class AP_HAL_AVR::APM2RCInput : public AP_HAL::RCInput {
public: /* Pass in a AP_HAL_AVR::ISRRegistry* as void*. */
APM2RCInput() : _init(0) {} void init(void* isrregistry);
void init(int machtnicht) { _init = 2; } uint8_t valid();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
private: 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__ #endif // __AP_HAL_AVR_RC_INPUT_H__

96
libraries/AP_HAL_AVR/RCInput_APM1.cpp Normal file
View File

@ -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 Normal file
View File

@ -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;
}