zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_Linux: Navio RCInput reimplemented.

master
Vladislav Zakharov 10 years ago committed by Andrew Tridgell
parent
e505eb2cd8
commit
be4b6033e4
2 changed files with 634 additions and 65 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. 561
      libraries/AP_HAL_Linux/RCInput_Navio.cpp
  2. 138
      libraries/AP_HAL_Linux/RCInput_Navio.h

561
libraries/AP_HAL_Linux/RCInput_Navio.cpp
Unescape View File

@ -1,77 +1,538 @@ @@ -1,77 +1,538 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#include "GPIO.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <pthread.h>
#include <string.h>
#include <errno.h>
#include <stdint.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <assert.h>
#include "RCInput_Navio.h"
//Parametres
#define RCIN_NAVIO_BUFFER_LENGTH 8
#define RCIN_NAVIO_SAMPLE_FREQ 500
#define RCIN_NAVIO_DMA_CHANNEL 0
#define RCIN_NAVIO_MAX_COUNTER 1300
#define PPM_INPUT_NAVIO RPI_GPIO_4
#define RCIN_NAVIO_MAX_SIZE_LINE 50
//Memory Addresses
#define RCIN_NAVIO_RPI1_DMA_BASE 0x20007000
#define RCIN_NAVIO_RPI1_CLK_BASE 0x20101000
#define RCIN_NAVIO_RPI1_PCM_BASE 0x20203000
#define RCIN_NAVIO_RPI2_DMA_BASE 0x3F007000
#define RCIN_NAVIO_RPI2_CLK_BASE 0x3F101000
#define RCIN_NAVIO_RPI2_PCM_BASE 0x3F203000
#define RCIN_NAVIO_GPIO_LEV0_ADDR 0x7e200034
#define RCIN_NAVIO_DMA_LEN 0x1000
#define RCIN_NAVIO_CLK_LEN 0xA8
#define RCIN_NAVIO_PCM_LEN 0x24
#define RCIN_NAVIO_TIMER_BASE 0x7e003004
#define RCIN_NAVIO_DMA_SRC_INC (1<<8)
#define RCIN_NAVIO_DMA_DEST_INC (1<<4)
#define RCIN_NAVIO_DMA_NO_WIDE_BURSTS (1<<26)
#define RCIN_NAVIO_DMA_WAIT_RESP (1<<3)
#define RCIN_NAVIO_DMA_D_DREQ (1<<6)
#define RCIN_NAVIO_DMA_PER_MAP(x) ((x)<<16)
#define RCIN_NAVIO_DMA_END (1<<1)
#define RCIN_NAVIO_DMA_RESET (1<<31)
#define RCIN_NAVIO_DMA_INT (1<<2)
#define RCIN_NAVIO_DMA_CS (0x00/4)
#define RCIN_NAVIO_DMA_CONBLK_AD (0x04/4)
#define RCIN_NAVIO_DMA_DEBUG (0x20/4)
#define RCIN_NAVIO_PCM_CS_A (0x00/4)
#define RCIN_NAVIO_PCM_FIFO_A (0x04/4)
#define RCIN_NAVIO_PCM_MODE_A (0x08/4)
#define RCIN_NAVIO_PCM_RXC_A (0x0c/4)
#define RCIN_NAVIO_PCM_TXC_A (0x10/4)
#define RCIN_NAVIO_PCM_DREQ_A (0x14/4)
#define RCIN_NAVIO_PCM_INTEN_A (0x18/4)
#define RCIN_NAVIO_PCM_INT_STC_A (0x1c/4)
#define RCIN_NAVIO_PCM_GRAY (0x20/4)
#define RCIN_NAVIO_PCMCLK_CNTL 38
#define RCIN_NAVIO_PCMCLK_DIV 39
extern const AP_HAL::HAL& hal;
using namespace Linux;
// This is a prototype code for RC input decoding on Raspberry Pi.
// It uses pigpio daemon to sample GPIOs over DMA with 1 microsecond resolution.
// This code should be rewritten to configure DMA GPIO sampling without pigpio.
void LinuxRCInput_Navio::init(void*)
volatile uint32_t *LinuxRCInput_Navio::pcm_reg;
volatile uint32_t *LinuxRCInput_Navio::clk_reg;
volatile uint32_t *LinuxRCInput_Navio::dma_reg;
Memory_table::Memory_table()
{
curtick = 0;
prevtick = 0;
width_s0 = 0;
width_s1 = 0;
// Kills pigpio daemon in case it was run with wrong parameters
unlink("/var/run/pigpio.pid");
system("killall pigpiod -q");
hal.scheduler->delay(1000);
// Starts pigpio daemon with 1 microsecond sampling resolution
system("pigpiod -s 1");
hal.scheduler->delay(1000);
// Configures pigpiod to send GPIO change notifications to /dev/pigpio0
system("pigs NO NB 0 0x10");
hal.scheduler->delay(1000);
_page_count = 0;
}
//Init Memory table
Memory_table::Memory_table(uint32_t page_count, int version)
{
uint32_t i;
int fdMem, file;
//Cache coherent adresses depends on RPI's version
uint32_t bus = version == 1 ? 0x40000000 : 0xC0000000;
uint64_t pageInfo;
void* offset;
_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);
}
if ((file = open("/proc/self/pagemap", O_RDWR | O_SYNC)) < 0) {
fprintf(stderr,"Failed to open /proc/self/pagemap\n");
exit(-1);
}
//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);
lseek(file, ((uint32_t)offset)/PAGE_SIZE*8, SEEK_SET);
//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);
_phys_pages[i] = (void*)((uint32_t)(pageInfo*PAGE_SIZE) | bus);
}
//Map physical addresses to virtual memory
for (i = 0; i < _page_count; i++) {
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, ((uint32_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus)));
memset(_virt_pages[i], 0xee, PAGE_SIZE);
}
close(file);
close(fdMem);
}
Memory_table::~Memory_table()
{
free(_virt_pages);
free(_phys_pages);
}
//Get physical address from the corresponding virtual adress from memory_table.
void* Memory_table::get_phys_addr(void* virt_addr)
{
uint32_t i;
for (i = 0; i < _page_count; i++) {
if ((uint32_t) _virt_pages[i] == (((uint32_t) virt_addr) & 0xFFFFF000)) {
return (void*) (((uint32_t) _phys_pages[i] + ((uint32_t) virt_addr & 0xFFF)));
}
}
return NULL;
}
//Get virtual address from the corresponding physical adress from memory_table.
void* Memory_table::get_virt_addr(void* phys_addr)
{
uint32_t i;
// phys_addr = (void*)((uint32_t) phys_addr);
for (i = 0; i < _page_count; i++) {
if ((uint32_t) _phys_pages[i] == (((uint32_t) phys_addr) & 0xFFFFF000)) {
return (void*) ((uint32_t) _virt_pages[i] + ((uint32_t) phys_addr & 0xFFF));
}
}
return NULL;
}
// This function returns virtual address with help of pointer, which is offset from the beginning of the buffer.
void* Memory_table::get_virt_from_pointer(uint32_t pointer)
{
if (pointer >= PAGE_SIZE * _page_count) {
return NULL;
}
return (uint8_t*)_virt_pages[(uint32_t) pointer / 4096] + pointer % 4096;
}
// This function returns physical address with help of pointer, which is offset from the beginning of the buffer.
void* Memory_table::get_phys_from_pointer(uint32_t pointer)
{
if (pointer >= PAGE_SIZE * _page_count) {
return NULL;
}
return (uint8_t*)_phys_pages[(uint32_t) pointer / 4096] + pointer % 4096;
}
// This function returns offset from the beginning of the buffer using virtual address and memory_table.
uint32_t Memory_table::get_pointer_from_virt(void* virt_addr)
{
uint32_t i;
for (i = 0; i < _page_count; i++) {
if ((uint32_t) _virt_pages[i] == (((uint32_t) virt_addr) & 0xFFFFF000)) {
return (i*PAGE_SIZE + ((uint32_t) virt_addr & 0xFFF));
}
}
return -1;
}
//How many bytes are available for reading in circle buffer?
uint32_t Memory_table::bytes_available(void* read_addr, void* write_addr)
{
if (write_addr > read_addr) {
return ((uint32_t) write_addr - (uint32_t) read_addr);
}
else {
return _page_count * PAGE_SIZE - ((uint32_t) read_addr - (uint32_t) write_addr);
}
}
uint32_t Memory_table::get_page_count()
{
return _page_count;
}
// More memory mapping
int get_raspberry_pi_version()
{
char buffer[RCIN_NAVIO_MAX_SIZE_LINE];
const char* hardware_description_entry = "Hardware";
const char* v1 = "BCM2708";
const char* v2 = "BCM2709";
char* flag;
FILE* fd;
fd = fopen("/proc/cpuinfo", "r");
while (fgets(buffer, RCIN_NAVIO_MAX_SIZE_LINE, fd) != NULL) {
flag = strstr(buffer, hardware_description_entry);
if (flag != NULL) {
if (strstr(buffer, v2) != NULL) {
printf("Raspberry Pi 2 with BCM2709!\n");
fclose(fd);
return 2;
}
else if (strstr(buffer, v1) != NULL) {
printf("Raspberry Pi 1 with BCM2708!\n");
fclose(fd);
return 1;
}
}
}
return 0;
}
//Physical addresses of peripheral depends on Raspberry Pi's version
void LinuxRCInput_Navio::set_physical_addresses(int version)
{
if (version == 1) {
dma_base = RCIN_NAVIO_RPI1_DMA_BASE;
clk_base = RCIN_NAVIO_RPI1_CLK_BASE;
pcm_base = RCIN_NAVIO_RPI1_PCM_BASE;
}
else if (version == 2) {
dma_base = RCIN_NAVIO_RPI2_DMA_BASE;
clk_base = RCIN_NAVIO_RPI2_CLK_BASE;
pcm_base = RCIN_NAVIO_RPI2_PCM_BASE;
}
}
//Map peripheral to virtual memory
void* LinuxRCInput_Navio::map_peripheral(uint32_t base, uint32_t len)
{
int fd = open("/dev/mem", O_RDWR);
void * vaddr;
if (fd < 0) {
printf("Failed to open /dev/mem: %m\n");
return NULL;
}
vaddr = mmap(NULL, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base);
if (vaddr == MAP_FAILED) {
printf("rpio-pwm: Failed to map peripheral at 0x%08x: %m\n", base);
}
close(fd);
return vaddr;
}
//Method to init DMA control block
void LinuxRCInput_Navio::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)->src = source;
(*cbp)->dst = dest;
(*cbp)->length = length;
(*cbp)->next = next_cb;
(*cbp)->stride = stride;
}
//Non-stoping DMA when the process is finished will lead to crash
void LinuxRCInput_Navio::stop_dma_and_exit(int param)
{
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = 0; // stop dma
exit(1);
}
//This function is used to init DMA control blocks (setting sampling GPIO register, destination adresses, synchronization)
void LinuxRCInput_Navio::init_ctrl_data()
{
uint32_t phys_fifo_addr;
uint64_t i;
uint32_t dest = 0;
uint32_t cbp = 0;
dma_cb_t* cbp_curr;
//Set fifo addr (for delay)
phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
// Configures /dev/pigpio0 for non-blocking read
//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.
*/
// fprintf(stderr, "ERROR SEARCH1\n");
for (i = 0; i < 56 * 128 * RCIN_NAVIO_BUFFER_LENGTH; i++)
{
//Transfer timer every 56th sample
if(i % 56 == 0){
cbp_curr = (dma_cb_t*)con_blocks->get_virt_from_pointer(cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP | RCIN_NAVIO_DMA_DEST_INC | RCIN_NAVIO_DMA_SRC_INC, RCIN_NAVIO_TIMER_BASE, (uint32_t) circle_buffer->get_phys_from_pointer(dest), 8, 0, (uint32_t) con_blocks->get_phys_from_pointer(cbp + sizeof(dma_cb_t)));
dest += 8;
cbp += sizeof(dma_cb_t);
}
// Transfer GPIO (1 byte)
cbp_curr = (dma_cb_t*)con_blocks->get_virt_from_pointer(cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP, RCIN_NAVIO_GPIO_LEV0_ADDR, (uint32_t) circle_buffer->get_phys_from_pointer(dest), 1, 0, (uint32_t) con_blocks->get_phys_from_pointer(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_virt_from_pointer(cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP | RCIN_NAVIO_DMA_D_DREQ | RCIN_NAVIO_DMA_PER_MAP(2), RCIN_NAVIO_TIMER_BASE, phys_fifo_addr, 4, 0, (uint32_t) con_blocks->get_phys_from_pointer(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_virt_from_pointer(cbp))->next = (uint32_t) con_blocks->get_phys_from_pointer(0);
}
/*Initialise PCM
See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/
void LinuxRCInput_Navio::init_PCM()
{
pcm_reg[RCIN_NAVIO_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz)
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_DIV] = 0x5A000000 | ((50000/RCIN_NAVIO_SAMPLE_FREQ)<<12); // Set pcm div. If we need to configure DMA frequency.
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_MODE_A] = (10 - 1) << 10; //PCM mode
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_DREQ_A] = 64<<24 | 64<<8; // DMA Req when one slot is free?
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<9; // Enable DMA
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<2; // Enable Tx
hal.scheduler->delay_microseconds(100);
}
/*Initialise DMA
See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/
void LinuxRCInput_Navio::init_DMA()
{
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = RCIN_NAVIO_DMA_RESET; //Reset DMA
hal.scheduler->delay_microseconds(100);
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = RCIN_NAVIO_DMA_INT | RCIN_NAVIO_DMA_END;
dma_reg[RCIN_NAVIO_DMA_CONBLK_AD | RCIN_NAVIO_DMA_CHANNEL << 8] = reinterpret_cast<uint32_t>(con_blocks->get_phys_from_pointer(0));//Set first control block address
dma_reg[RCIN_NAVIO_DMA_DEBUG | RCIN_NAVIO_DMA_CHANNEL << 8] = 7; // clear debug error flags
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = 0x10880001; // go, mid priority, wait for outstanding writes
}
//We must stop DMA when the process is killed
void LinuxRCInput_Navio::set_sigaction()
{
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));
sa.sa_handler = LinuxRCInput_Navio::stop_dma_and_exit;
sigaction(i, &sa, NULL);
}
}
//Initial setup of variables
LinuxRCInput_Navio::LinuxRCInput_Navio():
prev_tick(0),
delta_time(0),
curr_tick_inc(1000/RCIN_NAVIO_SAMPLE_FREQ),
curr_pointer(0),
curr_channel(0),
width_s0(0),
curr_signal(0),
last_signal(228),
state(RCIN_NAVIO_INITIAL_STATE)
{
int version = get_raspberry_pi_version();
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"
circle_buffer = new Memory_table(RCIN_NAVIO_BUFFER_LENGTH * 2, version);
con_blocks = new Memory_table(RCIN_NAVIO_BUFFER_LENGTH * 113, version);
}
LinuxRCInput_Navio::~LinuxRCInput_Navio()
{
delete circle_buffer;
delete con_blocks;
}
//Initializing necessary registers
void LinuxRCInput_Navio::init_registers()
{
dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_NAVIO_DMA_LEN);
pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_NAVIO_PCM_LEN);
clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_NAVIO_CLK_LEN);
}
void LinuxRCInput_Navio::init(void*)
{
pigpio = open("/dev/pigpio0", O_RDONLY);
init_registers();
if (pigpio == -1)
hal.scheduler->panic("No pigpio interface for RCInput");
//Enable PPM input
enable_pin = hal.gpio->channel(PPM_INPUT_NAVIO);
enable_pin->mode(HAL_GPIO_INPUT);
//Configuration
set_sigaction();
init_ctrl_data();
init_PCM();
init_DMA();
//wait a bit to let DMA fill queues and come to stable sampling
hal.scheduler->delay(300);
//Reading first sample
curr_tick = *((uint64_t*) circle_buffer->get_virt_from_pointer(curr_pointer));
prev_tick = curr_tick;
curr_pointer += 8;
curr_signal = *((uint8_t*) circle_buffer->get_virt_from_pointer(curr_pointer)) & 0x10 ? 1 : 0;
last_signal = curr_signal;
curr_pointer++;
}
//Processing signal
void LinuxRCInput_Navio::_timer_tick()
{
while (true) {
int bytesAvailable;
ioctl(pigpio, FIONREAD, &bytesAvailable);
if (bytesAvailable >= 12)
::read(pigpio, reinterpret_cast<uint8_t*>(&gpioReport), 12);
else
break;
prevtick = curtick;
curtick = gpioReport.tick;
if (((gpioReport.level >> 4) & 0x01) == 0) {
width_s0 = curtick - prevtick;
}
else {
width_s1 = curtick - prevtick;
_process_rc_pulse(width_s0, width_s1);
int j;
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((void*)dma_reg[RCIN_NAVIO_DMA_CONBLK_AD | RCIN_NAVIO_DMA_CHANNEL << 8]);
for(j = 1; j >= -1; j--){
x = circle_buffer->get_virt_addr((void*)(ad + j)->dst);
if(x != NULL) {
break;}
}
//How many bytes have DMA transfered (and we can process)?
counter = circle_buffer->bytes_available((void*)curr_pointer, (void*)circle_buffer->get_pointer_from_virt(x));
//We can't stay in method for a long time, because it may lead to delays
if (counter > RCIN_NAVIO_MAX_COUNTER) {
counter = RCIN_NAVIO_MAX_COUNTER;
}
//Processing ready bytes
for (;counter > 0x40;counter--) {
//Is it timer samle?
if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_virt_from_pointer(curr_pointer));
curr_pointer+=8;
counter-=8;
}
}
//Reading required bit
curr_signal = *((uint8_t*) circle_buffer->get_virt_from_pointer(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_NAVIO_INITIAL_STATE:
state = RCIN_NAVIO_ZERO_STATE;
break;
case RCIN_NAVIO_ZERO_STATE:
if (curr_signal == 0) {
width_s0 = (uint16_t) delta_time;
state = RCIN_NAVIO_ONE_STATE;
break;
}
else
break;
case RCIN_NAVIO_ONE_STATE:
if (curr_signal == 1) {
width_s1 = (uint16_t) delta_time;
state = RCIN_NAVIO_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) {
curr_pointer = 0;
}
curr_tick+=curr_tick_inc;
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE

138
libraries/AP_HAL_Linux/RCInput_Navio.h
Unescape View File

@ -1,31 +1,139 @@ @@ -1,31 +1,139 @@
#ifndef __AP_HAL_LINUX_RCINPUT_NAVIO_H__
#define __AP_HAL_LINUX_RCINPUT_NAVIO_H__
#include <AP_HAL_Linux.h>
#include "RCInput.h"
#include <signal.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <stdarg.h>
#include <stdint.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <assert.h>
#include <queue>
enum state_t{
RCIN_NAVIO_INITIAL_STATE = -1,
RCIN_NAVIO_ZERO_STATE = 0,
RCIN_NAVIO_ONE_STATE = 1
};
//Memory table structure
typedef struct {
void **virt_pages;
void **phys_pages;
uint32_t page_count;
} memory_table_t;
//DMA control block structure
typedef struct {
uint32_t info, src, dst, length,
stride, next, pad[2];
} dma_cb_t;
class Memory_table{
private:
void** _virt_pages;
void** _phys_pages;
uint32_t _page_count;
public:
Memory_table();
Memory_table(uint32_t, int);
~Memory_table();
//Get physical address from the corresponding virtual adress from memory_table.
void* get_phys_addr(void* virt_addr);
//Get virtual address from the corresponding physical adress from memory_table.
void* get_virt_addr(void* phys_addr);
// This function returns virtual address with help of pointer, which is offset from the beginning of the buffer.
void* get_virt_from_pointer(uint32_t pointer);
// This function returns physical address with help of pointer, which is offset from the beginning of the buffer.
void* get_phys_from_pointer(uint32_t pointer);
// This function returns offset from the beginning of the buffer using virtual address and memory_table.
uint32_t get_pointer_from_virt(void* virt_addr);
//How many bytes are available for reading in circle buffer?
uint32_t bytes_available(void* read_addr, void* write_addr);
uint32_t get_page_count();
};
class Linux::LinuxRCInput_Navio : public Linux::LinuxRCInput
{
public:
public:
void init(void*);
void _timer_tick(void);
LinuxRCInput_Navio();
~LinuxRCInput_Navio();
private:
int pigpio;
struct gpioReport_t
{
uint16_t seqno;
uint16_t flags;
uint32_t tick;
uint32_t level;
} gpioReport;
int curtick;
int prevtick;
private:
//Physical adresses of peripherals. Are different on different Raspberries.
uint32_t dma_base;
uint32_t clk_base;
uint32_t pcm_base;
//registers
static volatile uint32_t *pcm_reg;
static volatile uint32_t *clk_reg;
static volatile uint32_t *dma_reg;
Memory_table *circle_buffer;
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;
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);
void init_registers();
void init_ctrl_data();
void init_PCM();
void init_DMA();
void init_buffer();
static void stop_dma_and_exit(int param);
void set_sigaction();
void set_physical_addresses(int version);
};
#endif // __AP_HAL_LINUX_RCINPUT_NAVIO_H__

Write Preview
Loading…
Cancel
Save