Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_AVR RCInput: fixes found in testing 

* On APM1, I was able to test with the failsafe (i.e. all channels giving
  fixed values) but not with a real receiver
This commit is contained in:
Pat Hickey 2012-08-27 18:28:09 -07:00 committed by Andrew Tridgell
parent 71ea0fc755
commit 25bc452608
2 changed files with 38 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_HAL_AVR/RCInput_APM1.cpp
View File

@ -36,10 +36,11 @@ void APM1RCInput::_timer4_capt_cb(void) {
if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) { if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) {
_pulse_capt[channel_ctr] = pulse_width; _pulse_capt[channel_ctr] = pulse_width;
channel_ctr++; channel_ctr++;
} else { if (channel_ctr == AVR_RC_INPUT_NUM_CHANNELS) {
_valid = AVR_RC_INPUT_NUM_CHANNELS; _valid = AVR_RC_INPUT_NUM_CHANNELS;
} }
} }
}
icr4_prev = icr4_current; icr4_prev = icr4_current;
} }
@ -71,24 +72,35 @@ void APM1RCInput::init(void* _isrregistry) {
uint8_t APM1RCInput::valid() { return _valid; } uint8_t APM1RCInput::valid() { return _valid; }
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
/* constrain captured pulse to be between min and max pulsewidth. */
static inline uint16_t constrain_pulse(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;
}
uint16_t APM1RCInput::read(uint8_t ch) { uint16_t APM1RCInput::read(uint8_t ch) {
cli(); cli();
uint16_t capt = _pulse_capt[ch]; uint16_t capt = _pulse_capt[ch];
sei(); sei();
uint16_t capt_usec = capt >> 1;
_valid = 0; _valid = 0;
return constrain(capt_usec, /* scale _pulse_capt from 0.5us units to 1us units. */
RC_INPUT_MIN_PULSEWIDTH, RC_INPUT_MAX_PULSEWIDTH); return constrain_pulse(capt >> 1);
} }
uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) { uint8_t APM1RCInput::read(uint16_t* periods, uint8_t len) {
cli(); cli();
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
periods[i] = _pulse_capt[i] >> 1; periods[i] = _pulse_capt[i];
} }
sei(); sei();
/* Outside of critical section, do the math (in place) to scale and
* constrain the pulse. */
for (int i = 0; i < len; i++) {
/* scale _pulse_capt from 0.5us units to 1us units. */
periods[i] = constrain_pulse(periods[i] >> 1);
}
uint8_t v = _valid; uint8_t v = _valid;
_valid = 0; _valid = 0;
return v; return v;

24
libraries/AP_HAL_AVR/RCInput_APM2.cpp
View File

@ -36,10 +36,11 @@ void APM2RCInput::_timer5_capt_cb(void) {
if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) { if (channel_ctr < AVR_RC_INPUT_NUM_CHANNELS) {
_pulse_capt[channel_ctr] = pulse_width; _pulse_capt[channel_ctr] = pulse_width;
channel_ctr++; channel_ctr++;
} else { if (channel_ctr == AVR_RC_INPUT_NUM_CHANNELS) {
_valid = AVR_RC_INPUT_NUM_CHANNELS; _valid = AVR_RC_INPUT_NUM_CHANNELS;
} }
} }
}
icr5_prev = icr5_current; icr5_prev = icr5_current;
} }
@ -71,24 +72,35 @@ void APM2RCInput::init(void* _isrregistry) {
uint8_t APM2RCInput::valid() { return _valid; } uint8_t APM2RCInput::valid() { return _valid; }
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) /* constrain captured pulse to be between min and max pulsewidth. */
static inline uint16_t constrain_pulse(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;
}
uint16_t APM2RCInput::read(uint8_t ch) { uint16_t APM2RCInput::read(uint8_t ch) {
cli(); cli();
uint16_t capt = _pulse_capt[ch]; uint16_t capt = _pulse_capt[ch];
sei(); sei();
uint16_t capt_usec = capt >> 1;
_valid = 0; _valid = 0;
return constrain(capt_usec, /* scale _pulse_capt from 0.5us units to 1us units. */
RC_INPUT_MIN_PULSEWIDTH, RC_INPUT_MAX_PULSEWIDTH); return constrain_pulse(capt >> 1);
} }
uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) { uint8_t APM2RCInput::read(uint16_t* periods, uint8_t len) {
cli(); cli();
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
periods[i] = _pulse_capt[i] >> 1; periods[i] = _pulse_capt[i];
} }
sei(); sei();
/* Outside of critical section, do the math (in place) to scale and
* constrain the pulse. */
for (int i = 0; i < len; i++) {
/* scale _pulse_capt from 0.5us units to 1us units. */
periods[i] = constrain_pulse(periods[i] >> 1);
}
uint8_t v = _valid; uint8_t v = _valid;
_valid = 0; _valid = 0;
return v; return v;