From 2d3a62eae3361f3269ecff02d0152604a5e66f69 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 18 Jan 2016 20:04:14 -0200 Subject: [PATCH] AP_HAL_Linux: RCInput_RPI: revert PWM support This reverts support for RCInput via PWM. This is causing trouble in some RPI-based boards, receiving a SIGSEGV. Let's revert it for now and retry this later. This reverts commit 5629f38b2c8e47408e31096d7bf4ab3310445692. This reverts commit 51fd0b3d554327b173607d27819b1aefceb8ae2c. This reverts commit 79d56073f74d48ce315fe7474ece44b418efc45c. --- libraries/AP_HAL_Linux/RCInput.cpp | 36 +-- libraries/AP_HAL_Linux/RCInput.h | 7 +- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 295 ++++++++++++------------- libraries/AP_HAL_Linux/RCInput_RPI.h | 35 +-- 4 files changed, 155 insertions(+), 218 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index 2bba7ad948..b29d0cad77 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -42,11 +42,6 @@ uint8_t RCInput::num_channels() return _num_channels; } -void RCInput::set_num_channels(uint8_t num) -{ - _num_channels = num; -} - uint16_t RCInput::read(uint8_t ch) { new_rc_input = false; @@ -317,18 +312,10 @@ reset: memset(&dsm_state, 0, sizeof(dsm_state)); } -void RCInput::_process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1) -{ - if (channel < _num_channels) { - _pwm_values[channel] = width_s1; // range: 700 ~ 2300 - new_rc_input = true; - } -} - /* process a RC input pulse of the given width */ -void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel) +void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) { #if 0 // useful for debugging @@ -340,23 +327,14 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t c fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1); } #endif + // treat as PPM-sum + _process_ppmsum_pulse(width_s0 + width_s1); - if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) { + // treat as SBUS + _process_sbus_pulse(width_s0, width_s1); - // treat as PPM-sum - _process_ppmsum_pulse(width_s0 + width_s1); - - // treat as SBUS - _process_sbus_pulse(width_s0, width_s1); - - // treat as DSM - _process_dsm_pulse(width_s0, width_s1); - - } else { - - // treat as PWM - _process_pwm_pulse(channel, width_s0, width_s1); - } + // treat as DSM + _process_dsm_pulse(width_s0, width_s1); } /* diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index b3f4eff47e..2ea32483d3 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -5,7 +5,6 @@ #include "AP_HAL_Linux.h" #define LINUX_RC_INPUT_NUM_CHANNELS 16 -#define LINUX_RC_INPUT_CHANNEL_INVALID (999) class Linux::RCInput : public AP_HAL::RCInput { public: @@ -18,8 +17,6 @@ public: virtual void init(); bool new_input(); uint8_t num_channels(); - void set_num_channels(uint8_t num); - uint16_t read(uint8_t ch); uint8_t read(uint16_t* periods, uint8_t len); @@ -36,8 +33,7 @@ public: protected: - void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1, - uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID); + void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1); void _update_periods(uint16_t *periods, uint8_t len); private: @@ -49,7 +45,6 @@ public: void _process_ppmsum_pulse(uint16_t width); void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1); void _process_dsm_pulse(uint16_t width_s0, uint16_t width_s1); - void _process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1); /* override state */ uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index f57f6ecf86..c2b9cb978d 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -30,29 +30,13 @@ #define RCIN_RPI_BUFFER_LENGTH 8 #define RCIN_RPI_SAMPLE_FREQ 500 #define RCIN_RPI_DMA_CHANNEL 0 -#define RCIN_RPI_MAX_SIZE_LINE 50 - +#define RCIN_RPI_MAX_COUNTER 1300 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -// Same as the circle_buffer size -// See comments in "init_ctrl_data()" to understand values "2" -#define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2) -#define RCIN_RPI_SIG_HIGH 0 -#define RCIN_RPI_SIG_LOW 1 -// Each gpio stands for a rcinput channel, -// the first one in RcChnGpioTbl is channel 1 in receiver -static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { - RPI_GPIO_5, RPI_GPIO_6, RPI_GPIO_12, - RPI_GPIO_13, RPI_GPIO_19, RPI_GPIO_20, - RPI_GPIO_21, RPI_GPIO_26 -}; +#define PPM_INPUT_RPI RPI_GPIO_5 #else -#define RCIN_RPI_MAX_COUNTER 10432 -#define RCIN_RPI_SIG_HIGH 1 -#define RCIN_RPI_SIG_LOW 0 -static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { - RPI_GPIO_4 -}; +#define PPM_INPUT_RPI RPI_GPIO_4 #endif +#define RCIN_RPI_MAX_SIZE_LINE 50 //Memory Addresses #define RCIN_RPI_RPI1_DMA_BASE 0x20007000 @@ -70,7 +54,7 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { #define RCIN_RPI_TIMER_BASE 0x7e003004 #define RCIN_RPI_DMA_SRC_INC (1<<8) -#define RCIN_RPI_DMA_DEST_INC (1<<4) +#define RCIN_RPI_DMA_DEST_INC (1<<4) #define RCIN_RPI_DMA_NO_WIDE_BURSTS (1<<26) #define RCIN_RPI_DMA_WAIT_RESP (1<<3) #define RCIN_RPI_DMA_D_DREQ (1<<6) @@ -124,7 +108,7 @@ Memory_table::Memory_table(uint32_t page_count, int version) _virt_pages = (void**)malloc(page_count * sizeof(void*)); _phys_pages = (void**)malloc(page_count * sizeof(void*)); _page_count = page_count; - + if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC)) < 0) { fprintf(stderr,"Failed to open /dev/mem\n"); exit(-1); @@ -142,7 +126,7 @@ Memory_table::Memory_table(uint32_t page_count, int version) //Get list of available cache coherent physical addresses for (i = 0; i < _page_count; i++) { _virt_pages[i] = mmap(0, PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,-1,0); - ::read(file, &pageInfo, 8); + ::read(file, &pageInfo, 8); _phys_pages[i] = (void*)((uintptr_t)(pageInfo*PAGE_SIZE) | bus); } @@ -162,19 +146,20 @@ Memory_table::~Memory_table() free(_phys_pages); } -/* Return physical address with help of pointer, which is offset from the - * beginning of the buffer. */ +// This function returns physical address with help of pointer, which is offset from the beginning of the buffer. void* Memory_table::get_page(void** const pages, uint32_t addr) const { if (addr >= PAGE_SIZE * _page_count) { - return NULL; + return NULL; } return (uint8_t*)pages[(uint32_t) addr / 4096] + addr % 4096; } -// Get virtual address from the corresponding physical address from memory_table. -void *Memory_table::get_virt_addr(const uint32_t phys_addr) const +//Get virtual address from the corresponding physical address from memory_table. +void* Memory_table::get_virt_addr(const uint32_t phys_addr) const { + // FIXME: Can't the address be calculated directly? + // FIXME: if the address room in _phys_pages is not fragmented one may avoid a complete loop .. uint32_t i = 0; for (; i < _page_count; i++) { if ((uintptr_t) _phys_pages[i] == (((uintptr_t) phys_addr) & 0xFFFFF000)) { @@ -184,13 +169,14 @@ void *Memory_table::get_virt_addr(const uint32_t phys_addr) const return NULL; } -// Return offset from the beginning of the buffer using virtual address and memory_table. +// FIXME: in-congruent function style see above +// This function returns offset from the beginning of the buffer using virtual address and memory_table. uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) const { uint32_t i = 0; for (; i < _page_count; i++) { if ((uintptr_t) pages[i] == (addr & 0xFFFFF000) ) { - return (i * PAGE_SIZE + (addr & 0xFFF)); + return (i*PAGE_SIZE + (addr & 0xFFF)); } } return -1; @@ -201,7 +187,8 @@ uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t { if (write_addr > read_addr) { return (write_addr - read_addr); - } else { + } + else { return _page_count * PAGE_SIZE - (read_addr - write_addr); } } @@ -218,7 +205,8 @@ void RCInput_RPI::set_physical_addresses(int version) dma_base = RCIN_RPI_RPI1_DMA_BASE; clk_base = RCIN_RPI_RPI1_CLK_BASE; pcm_base = RCIN_RPI_RPI1_PCM_BASE; - } else if (version == 2) { + } + else if (version == 2) { dma_base = RCIN_RPI_RPI2_DMA_BASE; clk_base = RCIN_RPI_RPI2_CLK_BASE; pcm_base = RCIN_RPI_RPI2_PCM_BASE; @@ -276,68 +264,69 @@ void RCInput_RPI::init_ctrl_data() uint32_t cbp = 0; dma_cb_t* cbp_curr; //Set fifo addr (for delay) - phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; - + phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; + //Init dma control blocks. - /*We are transferring 8 bytes of GPIO register. Every 7th iteration we are - sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need - 7 * 8 + 8 = 64 bytes of buffer. Value 7 was selected specially to have a 64-byte "block" - TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64 - "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of - vitual pages for control blocks): for every 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO - sampling, 7 control blocks for setting frequency and 1 control block for sampling timer, so, - we need 7 + 7 + 1 = 15 control blocks. For integer value, we need 15 pages of control blocks. - Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control - blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. - So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO - and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer. + /*We are transferring 1 byte of GPIO register. Every 56th iteration we are + sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need + 56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block" + TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64 + "blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of + vitual pages for control blocks): for every 56 iterations (64 bytes of buffer) we need 56 control blocks for GPIO + sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so, + we need 56 + 56 + 1 = 113 control blocks. For integer value, we need 113 pages of control blocks. + Each control block length is 32 bytes. In 113 pages we will have (113 * 4096 / 32) = 113 * 128 control + blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. + So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO + and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer. */ // fprintf(stderr, "ERROR SEARCH1\n"); - uint32_t i = 0; - for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) { - //Transfer timer every 7th sample - if (i % 7 == 0) { - cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); + uint32_t i = 0; + for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344 + { + //Transfer timer every 56th sample + if(i % 56 == 0) { + cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); - init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, - (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), - 8, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, - cbp + sizeof(dma_cb_t) ) ); + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, + (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), + 8, + 0, + (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, + cbp + sizeof(dma_cb_t) ) ); + + dest += 8; + cbp += sizeof(dma_cb_t); + } - dest += 8; - cbp += sizeof(dma_cb_t); - } + // Transfer GPIO (1 byte) + cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, + (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), + 1, + 0, + (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, + cbp + sizeof(dma_cb_t) ) ); + + dest += 1; + cbp += sizeof(dma_cb_t); - // Transfer GPIO (8 bytes) - cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); - init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, - (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), - 8, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, - cbp + sizeof(dma_cb_t) ) ); - - dest += 8; - cbp += sizeof(dma_cb_t); - - // Delay (for setting sampling frequency) - /* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so, - each sample of GPIO is limited by writing to PCA queue. - */ - cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); - init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2), - RCIN_RPI_TIMER_BASE, phys_fifo_addr, - 4, - 0, - (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, - cbp + sizeof(dma_cb_t) ) ); - - cbp += sizeof(dma_cb_t); - } - //Make last control block point to the first (to make circle) + // Delay (for setting sampling frequency) + /* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so, + each sample of GPIO is limited by writing to PCA queue. + */ + cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2), + RCIN_RPI_TIMER_BASE, phys_fifo_addr, + 4, + 0, + (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, + cbp + sizeof(dma_cb_t) ) ); + + cbp += sizeof(dma_cb_t); + } + //Make last control block point to the first (to make circle) cbp -= sizeof(dma_cb_t); ((dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp))->next = (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, 0); } @@ -346,7 +335,7 @@ void RCInput_RPI::init_ctrl_data() /*Initialise PCM See BCM2835 documentation: http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf -*/ +*/ void RCInput_RPI::init_PCM() { pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block @@ -374,7 +363,7 @@ void RCInput_RPI::init_PCM() /*Initialise DMA See BCM2835 documentation: http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf -*/ +*/ void RCInput_RPI::init_DMA() { dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset DMA @@ -382,14 +371,14 @@ void RCInput_RPI::init_DMA() dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_INT | RCIN_RPI_DMA_END; dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8] = reinterpret_cast(con_blocks->get_page(con_blocks->_phys_pages, 0));//Set first control block address dma_reg[RCIN_RPI_DMA_DEBUG | RCIN_RPI_DMA_CHANNEL << 8] = 7; // clear debug error flags - dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0x10880001; // go, mid priority, wait for outstanding writes + dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0x10880001; // go, mid priority, wait for outstanding writes } //We must stop DMA when the process is killed void RCInput_RPI::set_sigaction() { - for (int i = 0; i < 64; i++) { + for (int i = 0; i < 64; i++) { //catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled struct sigaction sa; memset(&sa, 0, sizeof(sa)); @@ -400,20 +389,26 @@ void RCInput_RPI::set_sigaction() //Initial setup of variables RCInput_RPI::RCInput_RPI(): + prev_tick(0), + delta_time(0), curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_pointer(0), - curr_channel(0) -{ + curr_channel(0), + width_s0(0), + curr_signal(0), + last_signal(228), + state(RCIN_RPI_INITIAL_STATE) +{ #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 - int version = 2; + int version = 2; #else int version = UtilRPI::from(hal.util)->get_rpi_version(); #endif set_physical_addresses(version); - //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "15" + //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "113" circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version); - con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version); + con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version); } RCInput_RPI::~RCInput_RPI() @@ -430,22 +425,19 @@ void RCInput_RPI::deinit() //Initializing necessary registers void RCInput_RPI::init_registers() { - dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_LEN); + dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_LEN); pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN); clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN); } void RCInput_RPI::init() { - uint64_t signal_states = 0; - + init_registers(); - - //Enable PPM or PWM input - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]); - rc_channels[i].enable_pin->mode(HAL_GPIO_INPUT); - } + + //Enable PPM input + enable_pin = hal.gpio->channel(PPM_INPUT_RPI); + enable_pin->mode(HAL_GPIO_INPUT); //Configuration set_sigaction(); @@ -458,17 +450,11 @@ void RCInput_RPI::init() //Reading first sample curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); + prev_tick = curr_tick; curr_pointer += 8; - signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].prev_tick = curr_tick; - rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH - : RCIN_RPI_SIG_LOW; - rc_channels[i].last_signal = rc_channels[i].curr_signal; - } - curr_pointer += 8; - - set_num_channels(RCIN_RPI_CHN_NUM); + curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; + last_signal = curr_signal; + curr_pointer++; } @@ -476,15 +462,16 @@ void RCInput_RPI::init() void RCInput_RPI::_timer_tick() { int j; - void *x = nullptr; - uint64_t signal_states(0); + void* x; //Now we are getting address in which DMAC is writing at current moment dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]); - for (j = 1; j >= -1 && x == nullptr; j--) { - x = circle_buffer->get_virt_addr((ad + j)->dst); + for(j = 1; j >= -1; j--){ + x = circle_buffer->get_virt_addr((ad + j)->dst); + if(x != NULL) { + break;} } - + //How many bytes have DMA transfered (and we can process)? counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x)); //We can't stay in method for a long time, because it may lead to delays @@ -493,7 +480,7 @@ void RCInput_RPI::_timer_tick() } //Processing ready bytes - for (;counter > 0x40;) { + for (;counter > 0x40;counter--) { //Is it timer samle? if (curr_pointer % (64) == 0) { curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); @@ -501,48 +488,40 @@ void RCInput_RPI::_timer_tick() counter-=8; } //Reading required bit - signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); - for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { - rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH - : RCIN_RPI_SIG_LOW; - - //If the signal changed - if (rc_channels[i].curr_signal != rc_channels[i].last_signal) { - rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick; - rc_channels[i].prev_tick = curr_tick; - switch (rc_channels[i].state) { - case RCIN_RPI_INITIAL_STATE: - rc_channels[i].state = RCIN_RPI_ZERO_STATE; - break; - case RCIN_RPI_ZERO_STATE: - if (rc_channels[i].curr_signal == 0) { - rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time; - rc_channels[i].state = RCIN_RPI_ONE_STATE; - } - break; - case RCIN_RPI_ONE_STATE: - if (rc_channels[i].curr_signal == 1) { - rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time; - rc_channels[i].state = RCIN_RPI_ZERO_STATE; -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH - _process_rc_pulse(rc_channels[i].width_s0, - rc_channels[i].width_s1, i); -#else - _process_rc_pulse(rc_channels[i].width_s0, - rc_channels[i].width_s1); -#endif - } - break; + curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; + //If the signal changed + if (curr_signal != last_signal) { + delta_time = curr_tick - prev_tick; + prev_tick = curr_tick; + switch (state) { + case RCIN_RPI_INITIAL_STATE: + state = RCIN_RPI_ZERO_STATE; + break; + case RCIN_RPI_ZERO_STATE: + if (curr_signal == 0) { + width_s0 = (uint16_t) delta_time; + state = RCIN_RPI_ONE_STATE; + break; } + else + break; + case RCIN_RPI_ONE_STATE: + if (curr_signal == 1) { + width_s1 = (uint16_t) delta_time; + state = RCIN_RPI_ZERO_STATE; + _process_rc_pulse(width_s0, width_s1); + break; + } + else + break; } - rc_channels[i].last_signal = rc_channels[i].curr_signal; } - curr_pointer += 8; - counter -= 8; + last_signal = curr_signal; + curr_pointer++; if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { curr_pointer = 0; } - curr_tick += curr_tick_inc; + curr_tick+=curr_tick_inc; } } -#endif +#endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.h b/libraries/AP_HAL_Linux/RCInput_RPI.h index f4c5565eed..4e1e2a2747 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.h +++ b/libraries/AP_HAL_Linux/RCInput_RPI.h @@ -22,11 +22,6 @@ #include #include -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -#define RCIN_RPI_CHN_NUM 8 -#else -#define RCIN_RPI_CHN_NUM 1 -#endif enum state_t{ RCIN_RPI_INITIAL_STATE = -1, @@ -103,33 +98,23 @@ private: Memory_table *con_blocks; uint64_t curr_tick; + uint64_t prev_tick; + uint64_t delta_time; uint32_t curr_tick_inc; uint32_t curr_pointer; uint32_t curr_channel; uint32_t counter; - struct RcChannel { - RcChannel() : - prev_tick(0), delta_time(0), - width_s0(0), width_s1(0), - curr_signal(0), last_signal(0), - enable_pin(0), state(RCIN_RPI_INITIAL_STATE) - {} + uint16_t width_s0; + uint16_t width_s1; - uint64_t prev_tick; - uint64_t delta_time; - - uint16_t width_s0; - uint16_t width_s1; - - uint8_t curr_signal; - uint8_t last_signal; - - state_t state; - - AP_HAL::DigitalSource *enable_pin; - } rc_channels[RCIN_RPI_CHN_NUM]; + uint8_t curr_signal; + uint8_t last_signal; + + state_t state; + + AP_HAL::DigitalSource *enable_pin; void init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb); void* map_peripheral(uint32_t base, uint32_t len);