Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCInput_RPI: fix whitespaces

Browse Source 
Also add/change some minor coding style issues, reducing scope of
variables.
This commit is contained in:
Lucas De Marchi 2016-11-18 15:53:57 -02:00
parent aadc1643fc
commit 504de3ea9e
2 changed files with 138 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

276
libraries/AP_HAL_Linux/RCInput_RPI.cpp
View File

@ -95,45 +95,45 @@ Memory_table::Memory_table()
_page_count = 0; _page_count = 0;
} }
//Init Memory table // Init Memory table
Memory_table::Memory_table(uint32_t page_count, int version) Memory_table::Memory_table(uint32_t page_count, int version)
{ {
uint32_t i; uint32_t i;
int fdMem, file; int fdMem, file;
//Cache coherent adresses depends on RPI's version // Cache coherent adresses depends on RPI's version
uint32_t bus = version == 1 ? 0x40000000 : 0xC0000000; uint32_t bus = version == 1 ? 0x40000000 : 0xC0000000;
uint64_t pageInfo; uint64_t pageInfo;
void* offset; void *offset;
_virt_pages = (void**)malloc(page_count * sizeof(void*)); _virt_pages = (void **)malloc(page_count * sizeof(void *));
_phys_pages = (void**)malloc(page_count * sizeof(void*)); _phys_pages = (void **)malloc(page_count * sizeof(void *));
_page_count = page_count; _page_count = page_count;
if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC | O_CLOEXEC)) < 0) { if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC | O_CLOEXEC)) < 0) {
fprintf(stderr,"Failed to open /dev/mem\n"); fprintf(stderr, "Failed to open /dev/mem\n");
exit(-1); exit(-1);
} }
if ((file = open("/proc/self/pagemap", O_RDWR | O_SYNC | O_CLOEXEC)) < 0) { if ((file = open("/proc/self/pagemap", O_RDWR | O_SYNC | O_CLOEXEC)) < 0) {
fprintf(stderr,"Failed to open /proc/self/pagemap\n"); fprintf(stderr, "Failed to open /proc/self/pagemap\n");
exit(-1); exit(-1);
} }
//Magic to determine the physical address for this page: // Magic to determine the physical address for this page:
offset = mmap(0, _page_count*PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,-1,0); offset = mmap(0, _page_count * PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_ANONYMOUS | MAP_NORESERVE | MAP_LOCKED, -1, 0);
lseek(file, ((uintptr_t)offset)/PAGE_SIZE*8, SEEK_SET); lseek(file, ((uintptr_t)offset) / PAGE_SIZE * 8, SEEK_SET);
//Get list of available cache coherent physical addresses // Get list of available cache coherent physical addresses
for (i = 0; i < _page_count; i++) { 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); _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); _phys_pages[i] = (void *)((uintptr_t)(pageInfo * PAGE_SIZE) | bus);
} }
//Map physical addresses to virtual memory // Map physical addresses to virtual memory
for (i = 0; i < _page_count; i++) { for (i = 0; i < _page_count; i++) {
munmap(_virt_pages[i], PAGE_SIZE); munmap(_virt_pages[i], PAGE_SIZE);
_virt_pages[i] = mmap(_virt_pages[i], PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_FIXED|MAP_NORESERVE|MAP_LOCKED, fdMem, ((uintptr_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus))); _virt_pages[i] = mmap(_virt_pages[i], PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED | MAP_NORESERVE | MAP_LOCKED, fdMem, ((uintptr_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus)));
memset(_virt_pages[i], 0xee, PAGE_SIZE); memset(_virt_pages[i], 0xee, PAGE_SIZE);
} }
close(file); close(file);
@ -146,31 +146,33 @@ Memory_table::~Memory_table()
free(_phys_pages); free(_phys_pages);
} }
// This function returns 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
void* Memory_table::get_page(void** const pages, uint32_t addr) const // from the beginning of the buffer.
void *Memory_table::get_page(void **const pages, uint32_t addr) const
{ {
if (addr >= PAGE_SIZE * _page_count) { if (addr >= PAGE_SIZE * _page_count) {
return nullptr; return nullptr;
} }
return (uint8_t*)pages[(uint32_t) addr / 4096] + addr % 4096; return (uint8_t *)pages[(uint32_t)addr / 4096] + addr % 4096;
} }
//Get virtual address from the corresponding physical address from memory_table. //Get virtual address from the corresponding physical address from memory_table.
void* Memory_table::get_virt_addr(const uint32_t phys_addr) const void *Memory_table::get_virt_addr(const uint32_t phys_addr) const
{ {
// FIXME: Can't the address be calculated directly? // 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 .. // FIXME: if the address room in _phys_pages is not fragmented one may avoid
// a complete loop ..
uint32_t i = 0; uint32_t i = 0;
for (; i < _page_count; i++) { for (; i < _page_count; i++) {
if ((uintptr_t) _phys_pages[i] == (((uintptr_t) phys_addr) & 0xFFFFF000)) { if ((uintptr_t)_phys_pages[i] == (((uintptr_t)phys_addr) & 0xFFFFF000)) {
return (void*) ((uintptr_t) _virt_pages[i] + (phys_addr & 0xFFF)); return (void *)((uintptr_t)_virt_pages[i] + (phys_addr & 0xFFF));
} }
} }
return nullptr; return nullptr;
} }
// FIXME: in-congruent function style see above // This function returns offset from the beginning of the buffer using virtual
// This function returns offset from the beginning of the buffer using virtual address and memory_table. // address and memory_table.
uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) const uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) const
{ {
uint32_t i = 0; uint32_t i = 0;
@ -182,13 +184,12 @@ uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) cons
return -1; return -1;
} }
//How many bytes are available for reading in circle buffer? // How many bytes are available for reading in circle buffer?
uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t write_addr) const uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t write_addr) const
{ {
if (write_addr > read_addr) { if (write_addr > read_addr) {
return (write_addr - read_addr); return (write_addr - read_addr);
} } else {
else {
return _page_count * PAGE_SIZE - (read_addr - write_addr); return _page_count * PAGE_SIZE - (read_addr - write_addr);
} }
} }
@ -198,32 +199,31 @@ uint32_t Memory_table::get_page_count() const
return _page_count; return _page_count;
} }
//Physical addresses of peripheral depends on Raspberry Pi's version // Physical addresses of peripheral depends on Raspberry Pi's version
void RCInput_RPI::set_physical_addresses(int version) void RCInput_RPI::set_physical_addresses(int version)
{ {
if (version == 1) { if (version == 1) {
dma_base = RCIN_RPI_RPI1_DMA_BASE; dma_base = RCIN_RPI_RPI1_DMA_BASE;
clk_base = RCIN_RPI_RPI1_CLK_BASE; clk_base = RCIN_RPI_RPI1_CLK_BASE;
pcm_base = RCIN_RPI_RPI1_PCM_BASE; pcm_base = RCIN_RPI_RPI1_PCM_BASE;
} } else if (version == 2) {
else if (version == 2) {
dma_base = RCIN_RPI_RPI2_DMA_BASE; dma_base = RCIN_RPI_RPI2_DMA_BASE;
clk_base = RCIN_RPI_RPI2_CLK_BASE; clk_base = RCIN_RPI_RPI2_CLK_BASE;
pcm_base = RCIN_RPI_RPI2_PCM_BASE; pcm_base = RCIN_RPI_RPI2_PCM_BASE;
} }
} }
//Map peripheral to virtual memory // Map peripheral to virtual memory
void* RCInput_RPI::map_peripheral(uint32_t base, uint32_t len) void *RCInput_RPI::map_peripheral(uint32_t base, uint32_t len)
{ {
int fd = open("/dev/mem", O_RDWR | O_CLOEXEC); int fd = open("/dev/mem", O_RDWR | O_CLOEXEC);
void * vaddr; void *vaddr;
if (fd < 0) { if (fd < 0) {
printf("Failed to open /dev/mem: %m\n"); printf("Failed to open /dev/mem: %m\n");
return nullptr; return nullptr;
} }
vaddr = mmap(nullptr, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base); vaddr = mmap(nullptr, len, PROT_READ | PROT_WRITE, MAP_SHARED, fd, base);
if (vaddr == MAP_FAILED) { if (vaddr == MAP_FAILED) {
printf("rpio-pwm: Failed to map peripheral at 0x%08x: %m\n", base); printf("rpio-pwm: Failed to map peripheral at 0x%08x: %m\n", base);
} }
@ -232,8 +232,8 @@ void* RCInput_RPI::map_peripheral(uint32_t base, uint32_t len)
return vaddr; return vaddr;
} }
//Method to init DMA control block // Method to init DMA control block
void RCInput_RPI::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 RCInput_RPI::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)
{ {
(*cbp)->info = mode; (*cbp)->info = mode;
(*cbp)->src = source; (*cbp)->src = source;
@ -255,18 +255,18 @@ void RCInput_RPI::termination_handler(int signum)
AP_HAL::panic("Interrupted: %s", strsignal(signum)); AP_HAL::panic("Interrupted: %s", strsignal(signum));
} }
// This function is used to init DMA control blocks (setting sampling GPIO
//This function is used to init DMA control blocks (setting sampling GPIO register, destination adresses, synchronization) // register, destination adresses, synchronization)
void RCInput_RPI::init_ctrl_data() void RCInput_RPI::init_ctrl_data()
{ {
uint32_t phys_fifo_addr; uint32_t phys_fifo_addr;
uint32_t dest = 0; uint32_t dest = 0;
uint32_t cbp = 0; uint32_t cbp = 0;
dma_cb_t* cbp_curr; dma_cb_t *cbp_curr;
//Set fifo addr (for delay) // 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. // Init dma control blocks.
/*We are transferring 1 byte of GPIO register. Every 56th iteration we are /*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 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" 56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block"
@ -280,62 +280,58 @@ void RCInput_RPI::init_ctrl_data()
So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO
and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer. and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer.
*/ */
// fprintf(stderr, "ERROR SEARCH1\n");
uint32_t i = 0; for (uint32_t i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) {
for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344 //Transfer timer every 56th sample
{ if (i % 56 == 0) {
//Transfer timer every 56th sample cbp_curr = (dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages, cbp);
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, 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), (uintptr_t)circle_buffer->get_page(circle_buffer->_phys_pages, dest),
8, 8,
0, 0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t) ) ); cbp + sizeof(dma_cb_t)));
dest += 8;
cbp += sizeof(dma_cb_t);
}
// Transfer GPIO (1 byte) dest += 8;
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); cbp += sizeof(dma_cb_t);
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) // Transfer GPIO (1 byte)
/* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so, 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. 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); 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), 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, RCIN_RPI_TIMER_BASE, phys_fifo_addr,
4, 4,
0, 0,
(uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t) ) ); 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);
}
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);
}
/*Initialise PCM /*Initialise PCM
See BCM2835 documentation: See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/ */
void RCInput_RPI::init_PCM() void RCInput_RPI::init_PCM()
{ {
pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
@ -363,7 +359,7 @@ void RCInput_RPI::init_PCM()
/*Initialise DMA /*Initialise DMA
See BCM2835 documentation: See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/ */
void RCInput_RPI::init_DMA() void RCInput_RPI::init_DMA()
{ {
dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset DMA dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset DMA
@ -375,7 +371,7 @@ void RCInput_RPI::init_DMA()
} }
//We must stop DMA when the process is killed // We must stop DMA when the process is killed
void RCInput_RPI::set_sigaction() void RCInput_RPI::set_sigaction()
{ {
for (int i = 0; i < NSIG; i++) { for (int i = 0; i < NSIG; i++) {
@ -393,7 +389,7 @@ void RCInput_RPI::set_sigaction()
} }
} }
//Initial setup of variables // Initial setup of variables
RCInput_RPI::RCInput_RPI(): RCInput_RPI::RCInput_RPI():
circle_buffer{nullptr}, circle_buffer{nullptr},
con_blocks{nullptr}, con_blocks{nullptr},
@ -423,9 +419,9 @@ void RCInput_RPI::teardown()
//Initializing necessary registers //Initializing necessary registers
void RCInput_RPI::init_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); pcm_reg = (uint32_t *)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN);
clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN); clk_reg = (uint32_t *)map_peripheral(clk_base, RCIN_RPI_CLK_LEN);
} }
void RCInput_RPI::init() void RCInput_RPI::init()
@ -440,68 +436,74 @@ void RCInput_RPI::init()
con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version); con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version);
init_registers(); init_registers();
//Enable PPM input // Enable PPM input
enable_pin = hal.gpio->channel(PPM_INPUT_RPI); enable_pin = hal.gpio->channel(PPM_INPUT_RPI);
enable_pin->mode(HAL_GPIO_INPUT); enable_pin->mode(HAL_GPIO_INPUT);
//Configuration // Configuration
set_sigaction(); set_sigaction();
init_ctrl_data(); init_ctrl_data();
init_PCM(); init_PCM();
init_DMA(); init_DMA();
//wait a bit to let DMA fill queues and come to stable sampling // Wait a bit to let DMA fill queues and come to stable sampling
hal.scheduler->delay(300); hal.scheduler->delay(300);
//Reading first sample // Reading first sample
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); curr_tick = *((uint64_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
prev_tick = curr_tick; prev_tick = curr_tick;
curr_pointer += 8; curr_pointer += 8;
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; curr_signal = *((uint8_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
last_signal = curr_signal; last_signal = curr_signal;
curr_pointer++; curr_pointer++;
_initialized = true; _initialized = true;
} }
// Processing signal
//Processing signal
void RCInput_RPI::_timer_tick() void RCInput_RPI::_timer_tick()
{ {
int j; uint32_t counter = 0;
void* x;
if (!_initialized) { if (!_initialized) {
return; return;
} }
//Now we are getting address in which DMAC is writing at current moment // 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]); 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; j--){ for (int j = 1; j >= -1; j--) {
x = circle_buffer->get_virt_addr((ad + j)->dst); void *x = circle_buffer->get_virt_addr((ad + j)->dst);
if(x != nullptr) { if (x != nullptr) {
break;} counter = circle_buffer->bytes_available(curr_pointer,
circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x));
break;
}
} }
//How many bytes have DMA transferred (and we can process)? if (counter == 0) {
counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x)); return;
//We can't stay in method for a long time, because it may lead to delays }
// How many bytes have DMA transferred (and we can process)?
// We can't stay in method for a long time, because it may lead to delays
if (counter > RCIN_RPI_MAX_COUNTER) { if (counter > RCIN_RPI_MAX_COUNTER) {
counter = RCIN_RPI_MAX_COUNTER; counter = RCIN_RPI_MAX_COUNTER;
} }
//Processing ready bytes // Processing ready bytes
for (;counter > 0x40;counter--) { for (; counter > 0x40; counter--) {
//Is it timer samle? // Is it timer sample?
if (curr_pointer % (64) == 0) { if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); curr_tick = *((uint64_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
curr_pointer+=8; curr_pointer += 8;
counter-=8; counter -= 8;
} }
//Reading required bit
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; // Reading required bit
//If the signal changed 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) { if (curr_signal != last_signal) {
delta_time = curr_tick - prev_tick; delta_time = curr_tick - prev_tick;
prev_tick = curr_tick; prev_tick = curr_tick;
@ -511,29 +513,27 @@ void RCInput_RPI::_timer_tick()
break; break;
case RCIN_RPI_ZERO_STATE: case RCIN_RPI_ZERO_STATE:
if (curr_signal == 0) { if (curr_signal == 0) {
width_s0 = (uint16_t) delta_time; width_s0 = (uint16_t)delta_time;
state = RCIN_RPI_ONE_STATE; state = RCIN_RPI_ONE_STATE;
break;
} }
else break;
break;
case RCIN_RPI_ONE_STATE: case RCIN_RPI_ONE_STATE:
if (curr_signal == 1) { if (curr_signal == 1) {
width_s1 = (uint16_t) delta_time; width_s1 = (uint16_t)delta_time;
state = RCIN_RPI_ZERO_STATE; state = RCIN_RPI_ZERO_STATE;
_process_rc_pulse(width_s0, width_s1); _process_rc_pulse(width_s0, width_s1);
break;
} }
else break;
break;
} }
} }
last_signal = curr_signal; last_signal = curr_signal;
curr_pointer++; curr_pointer++;
if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { if (curr_pointer >= circle_buffer->get_page_count() * PAGE_SIZE) {
curr_pointer = 0; curr_pointer = 0;
} }
curr_tick+=curr_tick_inc; curr_tick += curr_tick_inc;
} }
} }
#endif // CONFIG_HAL_BOARD_SUBTYPE
#endif

1
libraries/AP_HAL_Linux/RCInput_RPI.h
View File

@ -104,7 +104,6 @@ private:
uint32_t curr_tick_inc; uint32_t curr_tick_inc;
uint32_t curr_pointer; uint32_t curr_pointer;
uint32_t curr_channel; uint32_t curr_channel;
uint32_t counter;
uint16_t width_s0; uint16_t width_s0;
uint16_t width_s1; uint16_t width_s1;