Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_AVR: RCInput has overrides

Browse Source
This commit is contained in:
Pat Hickey 2012-12-06 12:45:56 -08:00 committed by Andrew Tridgell
parent cb38651df5
commit 8d97596ea0
3 changed files with 107 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_HAL_AVR/RCInput.h
View File

@ -36,12 +36,31 @@ public:
*/
uint8_t read(uint16_t* periods, uint8_t len);
/**
* Overrides: these are really grody and don't belong here but we need
* them at the moment to make the port work.
* case v of:
* v == -1 -> no change to this channel
* v == 0 -> do not override this channel
* v > 0 -> set v as override.
*/
/* set_overrides: array starts at ch 0, for len channels */
bool set_overrides(int16_t *overrides, uint8_t len);
/* set_override: set just a specific channel */
bool set_override(uint8_t channel, int16_t override);
/* clear_overrides: equivelant to setting all overrides to 0 */
void clear_overrides();
private:
/* 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;
/* override state */
uint16_t _override[AVR_RC_INPUT_NUM_CHANNELS];
};
class AP_HAL_AVR::APM2RCInput : public AP_HAL::RCInput {
@ -50,12 +69,18 @@ class AP_HAL_AVR::APM2RCInput : public AP_HAL::RCInput {
uint8_t valid();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
private:
/* 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;
/* override state */
uint16_t _override[AVR_RC_INPUT_NUM_CHANNELS];
};
#endif // __AP_HAL_AVR_RC_INPUT_H__

42
libraries/AP_HAL_AVR/RCInput_APM1.cpp
View File

@ -48,6 +48,8 @@ void APM1RCInput::init(void* _isrregistry) {
ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry;
isrregistry->register_signal(ISR_REGISTRY_TIMER4_CAPT, _timer4_capt_cb);
/* initialize overrides */
clear_overrides();
/* Arduino pin 49 is ICP4 / PL0, timer 4 input capture */
hal.gpio->pinMode(49, GPIO_INPUT);
/**
@ -81,15 +83,24 @@ static inline uint16_t constrain_pulse(uint16_t p) {
}
uint16_t APM1RCInput::read(uint8_t ch) {
/* constrain ch */
if (ch < AVR_RC_INPUT_NUM_CHANNELS) return 0;
/* grab channel from isr's memory in critical section*/
cli();
uint16_t capt = _pulse_capt[ch];
sei();
_valid = 0;
/* scale _pulse_capt from 0.5us units to 1us units. */
return constrain_pulse(capt >> 1);
uint16_t pulse = constrain_pulse(capt >> 1);
/* Check for override */
uint16_t over = _override[ch];
return (over == 0) ? pulse : over;
}
uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) {
/* constrain len */
if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; }
/* grab channels from isr's memory in critical section */
cli();
for (int i = 0; i < len; i++) {
periods[i] = _pulse_capt[i];
@ -100,9 +111,38 @@ uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) {
for (int i = 0; i < len; i++) {
/* scale _pulse_capt from 0.5us units to 1us units. */
periods[i] = constrain_pulse(periods[i] >> 1);
/* check for override */
if (_override[i] != 0) {
periods[i] = _override[i];
}
}
uint8_t v = _valid;
_valid = 0;
return v;
}
bool APM1RCInput::set_overrides(int16_t *overrides, uint8_t len) {
bool res = false;
for (int i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool APM1RCInput::set_override(uint8_t channel, int16_t override) {
if (override < 0) return false; /* -1: no change. */
if (channel < AVR_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
_valid = 1;
return true;
}
}
return false;
}
void APM1RCInput::clear_overrides() {
for (int i = 0; i < AVR_RC_INPUT_NUM_CHANNELS; i++) {
_override[i] = 0;
}
}

42
libraries/AP_HAL_AVR/RCInput_APM2.cpp
View File

@ -48,6 +48,8 @@ void APM2RCInput::init(void* _isrregistry) {
ISRRegistry* isrregistry = (ISRRegistry*) _isrregistry;
isrregistry->register_signal(ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb);
/* initialize overrides */
clear_overrides();
/* Arduino pin 48 is ICP5 / PL1, timer 5 input capture */
hal.gpio->pinMode(48, GPIO_INPUT);
/**
@ -81,15 +83,24 @@ static inline uint16_t constrain_pulse(uint16_t p) {
uint16_t APM2RCInput::read(uint8_t ch) {
/* constrain ch */
if (ch < AVR_RC_INPUT_NUM_CHANNELS) return 0;
/* grab channel from isr's memory in critical section*/
cli();
uint16_t capt = _pulse_capt[ch];
sei();
_valid = 0;
/* scale _pulse_capt from 0.5us units to 1us units. */
return constrain_pulse(capt >> 1);
uint16_t pulse = constrain_pulse(capt >> 1);
/* Check for override */
uint16_t over = _override[ch];
return (over == 0) ? pulse : over;
}
uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) {
/* constrain len */
if (len > AVR_RC_INPUT_NUM_CHANNELS) { len = AVR_RC_INPUT_NUM_CHANNELS; }
/* grab channels from isr's memory in critical section */
cli();
for (int i = 0; i < len; i++) {
periods[i] = _pulse_capt[i];
@ -100,9 +111,38 @@ uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) {
for (int i = 0; i < len; i++) {
/* scale _pulse_capt from 0.5us units to 1us units. */
periods[i] = constrain_pulse(periods[i] >> 1);
/* check for override */
if (_override[i] != 0) {
periods[i] = _override[i];
}
}
uint8_t v = _valid;
_valid = 0;
return v;
}
bool APM2RCInput::set_overrides(int16_t *overrides, uint8_t len) {
bool res = false;
for (int i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool APM2RCInput::set_override(uint8_t channel, int16_t override) {
if (override < 0) return false; /* -1: no change. */
if (channel < AVR_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
_valid = 1;
return true;
}
}
return false;
}
void APM2RCInput::clear_overrides() {
for (int i = 0; i < AVR_RC_INPUT_NUM_CHANNELS; i++) {
_override[i] = 0;
}
}