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

HAL_Linux: don't accept less than 5 input channels

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;
LinuxRCInput::LinuxRCInput() :
new_rc_input(false),
_channel_counter(-1)
{}
new_rc_input(false)
{
ppm_state._channel_counter = -1;
}
void LinuxRCInput::init(void* machtnichts)
{
@ -47,8 +48,10 @@ uint16_t LinuxRCInput::read(uint8_t ch)
if (_override[ch]) {
return _override[ch];
}
return _pulse_capt[ch];
if (ch >= _num_channels) {
return 0;
}
return _pwm_values[ch];
}
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) {
// a long pulse indicates the end of a frame. Reset the
// 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;
_num_channels = _channel_counter;
}
_channel_counter = 0;
ppm_state._channel_counter = 0;
return;
}
if (_channel_counter == -1) {
if (ppm_state._channel_counter == -1) {
// we are not synchronised
return;
}
@ -125,18 +131,22 @@ void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec)
*/
if (width_usec > 700 && width_usec < 2300) {
// 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
_channel_counter++;
ppm_state._channel_counter++;
}
// if we have reached the maximum supported channels then
// 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;
_channel_counter = -1;
_num_channels = _channel_counter;
ppm_state._channel_counter = -1;
}
}
@ -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;
if (sbus_decode(bytes, values, &num_values,
&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++) {
_pulse_capt[i] = values[i];
_pwm_values[i] = values[i];
}
_num_channels = num_values;
new_rc_input = true;
}
goto reset;
} 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 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++) {
_pulse_capt[i] = values[i];
_pwm_values[i] = values[i];
}
_num_channels = num_values;
new_rc_input = true;
}
}
memset(&dsm_state, 0, sizeof(dsm_state));

13
libraries/AP_HAL_Linux/RCInput.h
View File

@ -28,12 +28,9 @@ public:
private:
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
int8_t _channel_counter;
uint16_t _pwm_values[LINUX_RC_INPUT_NUM_CHANNELS];
uint8_t _num_channels;
void _process_ppmsum_pulse(uint16_t width);
void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
@ -42,6 +39,12 @@ public:
/* override state */
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
struct {
uint16_t bytes[25]; // including start bit, parity and stop bits