From 5fb2ad0068b8cf88c581cb8bcd5f3a7eae85a932 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Aug 2014 12:32:22 +1000 Subject: [PATCH] HAL_Linux: fixed safety of RCInput code don't loop forever waiting for pulses --- libraries/AP_HAL_Linux/RCInput.cpp | 77 ++++++++++++++++++++++-------- libraries/AP_HAL_Linux/RCInput.h | 27 +++++++---- 2 files changed, 75 insertions(+), 29 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 4e9e4560f4..6129ab9602 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -18,13 +18,15 @@ using namespace Linux; LinuxRCInput::LinuxRCInput() : -new_rc_input(false) + new_rc_input(false), + _channel_counter(-1) {} void LinuxRCInput::init(void* machtnichts) { int mem_fd = open("/dev/mem", O_RDWR|O_SYNC); - ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, PRUSS_SHAREDRAM_BASE); + ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, + MAP_SHARED, mem_fd, RCIN_PRUSS_SHAREDRAM_BASE); close(mem_fd); ring_buffer->ring_head = 0; } @@ -95,28 +97,63 @@ void LinuxRCInput::clear_overrides() } } -void LinuxRCInput::_timer_tick() + +/* + process a pulse of the given width + */ +void LinuxRCInput::_process_pulse(uint16_t width_usec) { - uint8_t channel_ctr; - uint16_t s1_time, s2_time; - //scan for the latest start pulse - while(ring_buffer->buffer[ring_buffer->ring_head].delta_t < 8000){ - ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES; + 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) { + new_rc_input = true; + _num_channels = _channel_counter; + } + _channel_counter = 0; + return; + } + if (_channel_counter == -1) { + // we are not synchronised + return; + } + + // take a reading for the current channel + _pulse_capt[_channel_counter] = width_usec; + + // move to next channel + _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) { + new_rc_input = true; + _channel_counter = -1; + _num_channels = _channel_counter; } - for(channel_ctr = 0; channel_ctr < LINUX_RC_INPUT_NUM_CHANNELS; channel_ctr++){ - ring_buffer->ring_head = (ring_buffer->ring_head + 2) % NUM_RING_ENTRIES; - //wait until we receive two pulse_width(State1 & State2) values inside buffer - while(ring_buffer->ring_head <= (ring_buffer->ring_tail)); - s1_time = (uint32_t)ring_buffer->buffer[ring_buffer->ring_head - 1].delta_t; - s2_time = (uint32_t)ring_buffer->buffer[ring_buffer->ring_head].delta_t; - _pulse_capt[channel_ctr] = s1_time + s2_time; - if(_pulse_capt[channel_ctr] > RC_INPUT_MAX_PULSEWIDTH){ - _pulse_capt[channel_ctr] = RC_INPUT_MAX_PULSEWIDTH; +} + +/* + called at 1kHz to check for new pulse capture data from the PRU + */ +void LinuxRCInput::_timer_tick() +{ + while (ring_buffer->ring_head != ring_buffer->ring_tail) { + if (ring_buffer->ring_tail >= NUM_RING_ENTRIES) { + // invalid ring_tail from PRU - ignore RC input + return; } - else if(_pulse_capt[channel_ctr] < RC_INPUT_MIN_PULSEWIDTH){ - _pulse_capt[channel_ctr] = RC_INPUT_MIN_PULSEWIDTH; + if (ring_buffer->buffer[ring_buffer->ring_head].pin_value == 1) { + // remember the time we spent in the low state + _s0_time = ring_buffer->buffer[ring_buffer->ring_head].delta_t; + } else { + // the pulse value is the sum of the time spent in the low + // and high states + _process_pulse(ring_buffer->buffer[ring_buffer->ring_head].delta_t + _s0_time); } - } + // move to the next ring buffer entry + ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES; + } } #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index 7b2aa453dc..565f019791 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -4,10 +4,10 @@ #include -#define LINUX_RC_INPUT_NUM_CHANNELS 8 -#define PRUSS_SHAREDRAM_BASE 0x4a312000 -#define NUM_RING_ENTRIES 200 +#define LINUX_RC_INPUT_NUM_CHANNELS 16 +#define RCIN_PRUSS_SHAREDRAM_BASE 0x4a312000 +#define NUM_RING_ENTRIES 200 class Linux::LinuxRCInput : public AP_HAL::RCInput { public: @@ -21,25 +21,34 @@ public: bool set_overrides(int16_t *overrides, uint8_t len); bool set_override(uint8_t channel, int16_t override); void clear_overrides(); - + void _timer_tick(void); - + private: - bool new_rc_input; + volatile bool new_rc_input; uint16_t _pulse_capt[LINUX_RC_INPUT_NUM_CHANNELS]; uint8_t _num_channels; /* override state */ uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; + + // shared ring buffer with the PRU which records pin transitions struct ring_buffer { - volatile uint16_t ring_head; - volatile uint16_t ring_tail; - struct __attribute__((__packed__)) { + volatile uint16_t ring_head; // owned by ARM CPU + volatile uint16_t ring_tail; // owned by the PRU + struct { uint16_t pin_value; uint16_t delta_t; } buffer[NUM_RING_ENTRIES]; }; volatile struct ring_buffer *ring_buffer; + + // the channel we will receive input from next, or -1 when not synchronised + int8_t _channel_counter; + + // time spent in the low state + uint16_t _s0_time; + void _process_pulse(uint16_t width_usec); }; #endif // __AP_HAL_LINUX_RCINPUT_H__