zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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 5629f38b2c.
This reverts commit 51fd0b3d55.
This reverts commit 79d56073f7.
mission-4.1.18
Lucas De Marchi 9 years ago
parent
c8947c3e9c
commit
2d3a62eae3
4 changed files with 158 additions and 221 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 36
      libraries/AP_HAL_Linux/RCInput.cpp
  2. 7
      libraries/AP_HAL_Linux/RCInput.h
  3. 301
      libraries/AP_HAL_Linux/RCInput_RPI.cpp
  4. 35
      libraries/AP_HAL_Linux/RCInput_RPI.h

36
libraries/AP_HAL_Linux/RCInput.cpp
Unescape View File

@ -42,11 +42,6 @@ uint8_t RCInput::num_channels() @@ -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: @@ -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 @@ -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 PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
// treat as SBUS
_process_sbus_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);
}
/*

7
libraries/AP_HAL_Linux/RCInput.h
Unescape View File

@ -5,7 +5,6 @@ @@ -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: @@ -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: @@ -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: @@ -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];

301
libraries/AP_HAL_Linux/RCInput_RPI.cpp
Unescape View File

@ -30,29 +30,13 @@ @@ -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] = { @@ -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) @@ -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) @@ -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() @@ -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 @@ -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 @@ -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) @@ -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() @@ -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);
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);
}
// 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)
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) ) );
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);
// 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() @@ -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() @@ -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() @@ -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<uintptr_t>(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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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

35
libraries/AP_HAL_Linux/RCInput_RPI.h
Unescape View File

@ -22,11 +22,6 @@ @@ -22,11 +22,6 @@
#include <assert.h>
#include <queue>
#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: @@ -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);

Write Preview
Loading…
Cancel
Save