mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-18 06:38:29 -04:00
AP_HAL_Linux: RCInput_RPI: fix whitespaces
Also add/change some minor coding style issues, reducing scope of variables.
This commit is contained in:
Lucas De Marchi
parent
aadc1643fc
commit
504de3ea9e
@ -146,7 +146,8 @@ Memory_table::~Memory_table()
|
||||
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
|
||||
// from the beginning of the buffer.
|
||||
void *Memory_table::get_page(void **const pages, uint32_t addr) const
|
||||
{
|
||||
if (addr >= PAGE_SIZE * _page_count) {
|
||||
@ -159,7 +160,8 @@ void* Memory_table::get_page(void** const pages, uint32_t addr) const
|
||||
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 ..
|
||||
// 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)) {
|
||||
@ -169,8 +171,8 @@ void* Memory_table::get_virt_addr(const uint32_t phys_addr) const
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// FIXME: in-congruent function style see above
|
||||
// This function returns offset from the beginning of the buffer using virtual address and memory_table.
|
||||
// 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;
|
||||
@ -187,8 +189,7 @@ 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);
|
||||
}
|
||||
}
|
||||
@ -205,8 +206,7 @@ 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;
|
||||
@ -255,8 +255,8 @@ void RCInput_RPI::termination_handler(int signum)
|
||||
AP_HAL::panic("Interrupted: %s", strsignal(signum));
|
||||
}
|
||||
|
||||
|
||||
//This function is used to init DMA control blocks (setting sampling GPIO register, destination adresses, synchronization)
|
||||
// This function is used to init DMA control blocks (setting sampling GPIO
|
||||
// register, destination adresses, synchronization)
|
||||
void RCInput_RPI::init_ctrl_data()
|
||||
{
|
||||
uint32_t phys_fifo_addr;
|
||||
@ -280,11 +280,8 @@ void RCInput_RPI::init_ctrl_data()
|
||||
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 < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344
|
||||
{
|
||||
for (uint32_t i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) {
|
||||
//Transfer timer every 56th sample
|
||||
if (i % 56 == 0) {
|
||||
cbp_curr = (dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages, cbp);
|
||||
@ -331,7 +328,6 @@ void RCInput_RPI::init_ctrl_data()
|
||||
((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
|
||||
See BCM2835 documentation:
|
||||
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
|
||||
@ -451,7 +447,7 @@ void RCInput_RPI::init()
|
||||
init_PCM();
|
||||
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);
|
||||
|
||||
// Reading first sample
|
||||
@ -465,12 +461,10 @@ void RCInput_RPI::init()
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
|
||||
// Processing signal
|
||||
void RCInput_RPI::_timer_tick()
|
||||
{
|
||||
int j;
|
||||
void* x;
|
||||
uint32_t counter = 0;
|
||||
|
||||
if (!_initialized) {
|
||||
return;
|
||||
@ -478,14 +472,20 @@ void RCInput_RPI::_timer_tick()
|
||||
|
||||
// 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; j--){
|
||||
x = circle_buffer->get_virt_addr((ad + j)->dst);
|
||||
for (int j = 1; j >= -1; j--) {
|
||||
void *x = circle_buffer->get_virt_addr((ad + j)->dst);
|
||||
if (x != nullptr) {
|
||||
break;}
|
||||
counter = circle_buffer->bytes_available(curr_pointer,
|
||||
circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x));
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (counter == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// How many bytes have DMA transferred (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
|
||||
if (counter > RCIN_RPI_MAX_COUNTER) {
|
||||
counter = RCIN_RPI_MAX_COUNTER;
|
||||
@ -493,14 +493,16 @@ void RCInput_RPI::_timer_tick()
|
||||
|
||||
// Processing ready bytes
|
||||
for (; counter > 0x40; counter--) {
|
||||
//Is it timer samle?
|
||||
// Is it timer sample?
|
||||
if (curr_pointer % (64) == 0) {
|
||||
curr_tick = *((uint64_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
|
||||
curr_pointer += 8;
|
||||
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;
|
||||
@ -513,21 +515,18 @@ void RCInput_RPI::_timer_tick()
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
last_signal = curr_signal;
|
||||
curr_pointer++;
|
||||
if (curr_pointer >= circle_buffer->get_page_count() * PAGE_SIZE) {
|
||||
@ -536,4 +535,5 @@ void RCInput_RPI::_timer_tick()
|
||||
curr_tick += curr_tick_inc;
|
||||
}
|
||||
}
|
||||
#endif // CONFIG_HAL_BOARD_SUBTYPE
|
||||
|
||||
#endif
|
||||
|
||||
@ -104,7 +104,6 @@ private:
|
||||
uint32_t curr_tick_inc;
|
||||
uint32_t curr_pointer;
|
||||
uint32_t curr_channel;
|
||||
uint32_t counter;
|
||||
|
||||
uint16_t width_s0;
|
||||
uint16_t width_s1;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user