diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 528630c771..2946785090 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -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)); diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index 1489d84c9f..ac963d5281 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -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