diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index 465e8c7e41f..17550762dc9 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -18,6 +18,7 @@ namespace Linux { class LinuxGPIO; class LinuxDigitalSource; class LinuxRCInput; + class LinuxRCInput_PRU; class LinuxRCOutput; class LinuxSemaphore; class LinuxScheduler; diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index e86f7177310..cef7a3ef553 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -22,7 +22,11 @@ static LinuxSPIDeviceManager spiDeviceManager; static LinuxAnalogIn analogIn; static LinuxStorage storageDriver; static LinuxGPIO gpioDriver; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE +static LinuxRCInput_PRU rcinDriver; +#else static LinuxRCInput rcinDriver; +#endif static LinuxRCOutput rcoutDriver; static LinuxScheduler schedulerInstance; static LinuxUtil utilInstance; diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 6129ab9602f..c5d42d24096 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -17,6 +17,7 @@ #include "RCInput.h" using namespace Linux; + LinuxRCInput::LinuxRCInput() : new_rc_input(false), _channel_counter(-1) @@ -24,11 +25,6 @@ LinuxRCInput::LinuxRCInput() : 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, RCIN_PRUSS_SHAREDRAM_BASE); - close(mem_fd); - ring_buffer->ring_head = 0; } bool LinuxRCInput::new_input() @@ -101,7 +97,7 @@ void LinuxRCInput::clear_overrides() /* process a pulse of the given width */ -void LinuxRCInput::_process_pulse(uint16_t width_usec) +void LinuxRCInput::_process_ppmsum_pulse(uint16_t width_usec) { if (width_usec >= 4000) { // a long pulse indicates the end of a frame. Reset the @@ -133,27 +129,4 @@ void LinuxRCInput::_process_pulse(uint16_t width_usec) } } -/* - 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; - } - 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 565f019791d..d1110a44e2c 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -6,13 +6,10 @@ #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: LinuxRCInput(); - void init(void* machtnichts); + virtual void init(void* machtnichts); bool new_input(); uint8_t num_channels(); uint16_t read(uint8_t ch); @@ -22,33 +19,26 @@ public: bool set_override(uint8_t channel, int16_t override); void clear_overrides(); - void _timer_tick(void); + // default empty _timer_tick, this is overridden by board + // specific implementations + virtual void _timer_tick() {} + + protected: + void _process_ppmsum_pulse(uint16_t width_usec); private: 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; // 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); + /* override state */ + uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; }; +#include "RCInput_PRU.h" + #endif // __AP_HAL_LINUX_RCINPUT_H__ diff --git a/libraries/AP_HAL_Linux/RCInput_PRU.cpp b/libraries/AP_HAL_Linux/RCInput_PRU.cpp new file mode 100644 index 00000000000..303d0a4b166 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_PRU.cpp @@ -0,0 +1,59 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "RCInput.h" + +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +void LinuxRCInput_PRU::init(void*) +{ + int mem_fd = open("/dev/mem", O_RDWR|O_SYNC); + if (mem_fd == -1) { + hal.scheduler->panic("Unable to open /dev/mem"); + } + 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; + _s0_time = 0; +} + +/* + called at 1kHz to check for new pulse capture data from the PRU + */ +void LinuxRCInput_PRU::_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; + } + 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_ppmsum_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_PRU.h b/libraries/AP_HAL_Linux/RCInput_PRU.h new file mode 100644 index 00000000000..4aaba421854 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput_PRU.h @@ -0,0 +1,37 @@ + +#ifndef __AP_HAL_LINUX_RCINPUT_PRU_H__ +#define __AP_HAL_LINUX_RCINPUT_PRU_H__ + +/* + This class implements RCInput on the BeagleBoneBlack with a PRU + doing the edge detection of the PPM sum input + */ + +#include + +#define RCIN_PRUSS_SHAREDRAM_BASE 0x4a312000 +#define NUM_RING_ENTRIES 200 + +class Linux::LinuxRCInput_PRU : public Linux::LinuxRCInput +{ +public: + void init(void*); + void _timer_tick(void); + + private: + // shared ring buffer with the PRU which records pin transitions + struct ring_buffer { + 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; + + // time spent in the low state + uint16_t _s0_time; +}; + +#endif // __AP_HAL_LINUX_RCINPUT_PRU_H__