From 79d56073f74d48ce315fe7474ece44b418efc45c Mon Sep 17 00:00:00 2001 From: Aaron Wang Shi Date: Sun, 10 Jan 2016 19:52:44 +0800 Subject: [PATCH] AP_HAL_Linux: support PWM input for BH hat --- libraries/AP_HAL_Linux/RCInput.cpp | 36 ++++- libraries/AP_HAL_Linux/RCInput.h | 7 +- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 182 +++++++++++++++---------- libraries/AP_HAL_Linux/RCInput_RPI.h | 35 +++-- 4 files changed, 168 insertions(+), 92 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp index b29d0cad77..2bba7ad948 100644 --- a/libraries/AP_HAL_Linux/RCInput.cpp +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -42,6 +42,11 @@ 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; @@ -312,10 +317,18 @@ 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) +void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel) { #if 0 // useful for debugging @@ -327,14 +340,23 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1); } #endif - // treat as PPM-sum - _process_ppmsum_pulse(width_s0 + width_s1); - // treat as SBUS - _process_sbus_pulse(width_s0, width_s1); + if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) { + + // 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); + // treat as DSM + _process_dsm_pulse(width_s0, width_s1); + + } else { + + // treat as PWM + _process_pwm_pulse(channel, width_s0, width_s1); + } } /* diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h index 2ea32483d3..b3f4eff47e 100644 --- a/libraries/AP_HAL_Linux/RCInput.h +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -5,6 +5,7 @@ #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: @@ -17,6 +18,8 @@ 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); @@ -33,7 +36,8 @@ public: protected: - void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1); + void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1, + uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID); void _update_periods(uint16_t *periods, uint8_t len); private: @@ -45,6 +49,7 @@ 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 74ddeac4cd..48ab957c71 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -30,13 +30,29 @@ #define RCIN_RPI_BUFFER_LENGTH 8 #define RCIN_RPI_SAMPLE_FREQ 500 #define RCIN_RPI_DMA_CHANNEL 0 -#define RCIN_RPI_MAX_COUNTER 1300 +#define RCIN_RPI_MAX_SIZE_LINE 50 + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -#define PPM_INPUT_RPI RPI_GPIO_5 +// 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 +}; #else -#define PPM_INPUT_RPI RPI_GPIO_4 -#endif -#define RCIN_RPI_MAX_SIZE_LINE 50 +#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 +}; +#endif // CONFIG_HAL_BOARD_SUBTYPE //Memory Addresses #define RCIN_RPI_RPI1_DMA_BASE 0x20007000 @@ -267,26 +283,26 @@ void RCInput_RPI::init_ctrl_data() phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; //Init dma control blocks. - /*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. + /*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. */ // fprintf(stderr, "ERROR SEARCH1\n"); uint32_t i = 0; - for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344 + for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168 { - //Transfer timer every 56th sample - if(i % 56 == 0) { + //Transfer timer every 7th sample + if(i % 7 == 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, @@ -300,16 +316,16 @@ void RCInput_RPI::init_ctrl_data() cbp += sizeof(dma_cb_t); } - // Transfer GPIO (1 byte) + // 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), - 1, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, + 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 += 1; + + dest += 8; cbp += sizeof(dma_cb_t); // Delay (for setting sampling frequency) @@ -389,15 +405,9 @@ 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), - width_s0(0), - curr_signal(0), - last_signal(228), - state(RCIN_RPI_INITIAL_STATE) + curr_channel(0) { #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 int version = 2; @@ -406,9 +416,9 @@ RCInput_RPI::RCInput_RPI(): #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 "113" + //Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "15" circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version); - con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version); + con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version); } RCInput_RPI::~RCInput_RPI() @@ -432,12 +442,15 @@ void RCInput_RPI::init_registers() void RCInput_RPI::init() { - + uint64_t signal_states(0); + init_registers(); - - //Enable PPM input - enable_pin = hal.gpio->channel(PPM_INPUT_RPI); - enable_pin->mode(HAL_GPIO_INPUT); + + //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); + } //Configuration set_sigaction(); @@ -450,11 +463,17 @@ 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; - curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; - last_signal = curr_signal; - curr_pointer++; + 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); } @@ -463,6 +482,7 @@ void RCInput_RPI::_timer_tick() { int j; void* x; + uint64_t signal_states(0); //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]); @@ -480,7 +500,7 @@ void RCInput_RPI::_timer_tick() } //Processing ready bytes - for (;counter > 0x40;counter--) { + for (;counter > 0x40;) { //Is it timer samle? if (curr_pointer % (64) == 0) { curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); @@ -488,36 +508,50 @@ void RCInput_RPI::_timer_tick() counter-=8; } //Reading required bit - 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; + 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; + } + else { + 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 // CONFIG_HAL_BOARD_SUBTYPE + break; + } + else{ + break; + } } - else - break; } + rc_channels[i].last_signal = rc_channels[i].curr_signal; } - last_signal = curr_signal; - curr_pointer++; + curr_pointer += 8; + counter -= 8; if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { curr_pointer = 0; } diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.h b/libraries/AP_HAL_Linux/RCInput_RPI.h index 4e1e2a2747..f4c5565eed 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.h +++ b/libraries/AP_HAL_Linux/RCInput_RPI.h @@ -22,6 +22,11 @@ #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, @@ -98,23 +103,33 @@ 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; - uint16_t width_s0; - uint16_t width_s1; + 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) + {} - uint8_t curr_signal; - uint8_t last_signal; - - state_t state; - - AP_HAL::DigitalSource *enable_pin; + 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]; 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);