aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_Linux: don't accept less than 5 input channels

Browse Source
This commit is contained in:
Andrew Tridgell 2014-10-09 09:19:35 +11:00
parent 1721216019
commit cebfef3ead
2 changed files with 40 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_HAL_Linux/RCInput.cpp
View File

@ -23,9 +23,10 @@ extern const AP_HAL::HAL& hal;
using namespace Linux; using namespace Linux;
LinuxRCInput::LinuxRCInput() : LinuxRCInput::LinuxRCInput() :
new_rc_input(false), new_rc_input(false)
_channel_counter(-1) {
{} ppm_state._channel_counter = -1;
}
void LinuxRCInput::init(void* machtnichts) void LinuxRCInput::init(void* machtnichts)
{ {
@ -47,8 +48,10 @@ uint16_t LinuxRCInput::read(uint8_t ch)
if (_override[ch]) { if (_override[ch]) {
return _override[ch]; return _override[ch];
} }
if (ch >= _num_channels) {
return _pulse_capt[ch]; return 0;
}
return _pwm_values[ch];
} }
uint8_t LinuxRCInput::read(uint16_t* periods, uint8_t len) uint8_t LinuxRCInput::read(uint16_t* periods, uint8_t len)
@ -106,14 +109,17 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
if (width_usec >= 4000) { if (width_usec >= 4000) {
// a long pulse indicates the end of a frame. Reset the // a long pulse indicates the end of a frame. Reset the
// channel counter so next pulse is channel 0 // channel counter so next pulse is channel 0
if (_channel_counter != -1) { if (ppm_state._channel_counter >= 5) {
for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
_pwm_values[i] = ppm_state._pulse_capt[i];
}
_num_channels = ppm_state._channel_counter;
new_rc_input = true; new_rc_input = true;
_num_channels = _channel_counter;
} }
_channel_counter = 0; ppm_state._channel_counter = 0;
return; return;
} }
if (_channel_counter == -1) { if (ppm_state._channel_counter == -1) {
// we are not synchronised // we are not synchronised
return; return;
} }
@ -125,18 +131,22 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
*/ */
if (width_usec > 700 && width_usec < 2300) { if (width_usec > 700 && width_usec < 2300) {
// take a reading for the current channel // take a reading for the current channel
_pulse_capt[_channel_counter] = width_usec; // buffer these
ppm_state._pulse_capt[ppm_state._channel_counter] = width_usec;
// move to next channel // move to next channel
_channel_counter++; ppm_state._channel_counter++;
} }
// if we have reached the maximum supported channels then // if we have reached the maximum supported channels then
// mark as unsynchronised, so we wait for a wide pulse // mark as unsynchronised, so we wait for a wide pulse
if (_channel_counter >= LINUX_RC_INPUT_NUM_CHANNELS) { if (ppm_state._channel_counter >= LINUX_RC_INPUT_NUM_CHANNELS) {
for (uint8_t i=0; i<ppm_state._channel_counter; i++) {
_pwm_values[i] = ppm_state._pulse_capt[i];
}
_num_channels = ppm_state._channel_counter;
new_rc_input = true; new_rc_input = true;
_channel_counter = -1; ppm_state._channel_counter = -1;
_num_channels = _channel_counter;
} }
} }
@ -206,11 +216,13 @@ void LinuxRCInput::_process_sbus_pulse(uint16_t width_s0, uint16_t width_s1)
bool sbus_failsafe=false, sbus_frame_drop=false; bool sbus_failsafe=false, sbus_frame_drop=false;
if (sbus_decode(bytes, values, &num_values, if (sbus_decode(bytes, values, &num_values,
&sbus_failsafe, &sbus_frame_drop, &sbus_failsafe, &sbus_frame_drop,
LINUX_RC_INPUT_NUM_CHANNELS)) { LINUX_RC_INPUT_NUM_CHANNELS) &&
num_values >= 5) {
for (i=0; i<num_values; i++) { for (i=0; i<num_values; i++) {
_pulse_capt[i] = values[i]; _pwm_values[i] = values[i];
} }
_num_channels = num_values; _num_channels = num_values;
new_rc_input = true;
} }
goto reset; goto reset;
} else if (bits_s1 > 12) { } else if (bits_s1 > 12) {
@ -268,11 +280,13 @@ void LinuxRCInput::_process_dsm_pulse(uint16_t width_s0, uint16_t width_s1)
} }
uint16_t values[8]; uint16_t values[8];
uint16_t num_values=0; uint16_t num_values=0;
if (dsm_decode(hal.scheduler->micros64(), bytes, values, &num_values, 8)) { if (dsm_decode(hal.scheduler->micros64(), bytes, values, &num_values, 8) &&
num_values >= 5) {
for (i=0; i<num_values; i++) { for (i=0; i<num_values; i++) {
_pulse_capt[i] = values[i]; _pwm_values[i] = values[i];
} }
_num_channels = num_values; _num_channels = num_values;
new_rc_input = true;
} }
} }
memset(&dsm_state, 0, sizeof(dsm_state)); memset(&dsm_state, 0, sizeof(dsm_state));

13
libraries/AP_HAL_Linux/RCInput.h
View File

@ -28,12 +28,9 @@ public:
private: private:
volatile bool new_rc_input; volatile bool new_rc_input;
uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS];
uint8_t _num_channels;
// the channel we will receive input from next, or -1 when not synchronised uint16_t _pwm_values[LINUX_RC_INPUT_NUM_CHANNELS];
int8_t _channel_counter; uint8_t _num_channels;
void _process_ppmsum_pulse(uint16_t width); void _process_ppmsum_pulse(uint16_t width);
void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1); void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
@ -42,6 +39,12 @@ public:
/* override state */ /* override state */
uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];
// state of ppm decoder
struct {
int8_t _channel_counter;
uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS];
} ppm_state;
// state of SBUS bit decoder // state of SBUS bit decoder
struct { struct {
uint16_t bytes[25]; // including start bit, parity and stop bits uint16_t bytes[25]; // including start bit, parity and stop bits