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

linux: RCInput_RPI RPI-4 Support

Browse Source
This commit is contained in:
MHefny 2021-04-04 11:39:20 +02:00 committed by Lucas De Marchi
parent 7b9826cdce
commit c57764e9b4
2 changed files with 60 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
libraries/AP_HAL_Linux/RCInput_RPI.cpp
View File

@ -102,8 +102,19 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
#define RCIN_RPI_PCM_INT_STC_A (0x1c/4)
#define RCIN_RPI_PCM_GRAY (0x20/4)
#define RCIN_RPI_PCMCLK_CNTL 38
#define RCIN_RPI_PCMCLK_DIV 39
#define RCIN_RPI_PCMCLK_CNTL 0x26
#define RCIN_RPI_PCMCLK_DIV 0x27
#define RCIN_RPI_PLL_CLK 50000
/*
* Defaults to 107.14MHz in the firmware setup.
* The PLL channel that used to generate the 100MHz now runs at 750MHz instead of 500,
* so the fw uses 750/7.
*
* see: https://www.raspberrypi.org/forums/viewtopic.php?t=251902
*/
#define RCIN_RPI4_PLL_CLK 70000
extern const AP_HAL::HAL& hal;
@ -355,23 +366,63 @@ 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);
}
void RCInput_RPI::init_PCM()
{
if (_version != 4)
{
init_PCM_BCM2835();
}
else
{
init_PCM_BCM2711();
}
}
/*Initialise PCM
See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/
void RCInput_RPI::init_PCM()
void RCInput_RPI::init_PCM_BCM2835()
{
pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz)
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((50000/RCIN_RPI_SAMPLE_FREQ)<<12); // Set pcm div. If we need to configure DMA frequency.
clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<<12); // Set pcm div. If we need to configure DMA frequency.
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10; //PCM mode
pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10; //PCM mode 0b0010010000000000
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_DREQ_A] = 64<<24 | 64<<8; // DMA Req when one slot is free?
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<9; // Enable DMA
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<2; // Enable Tx
hal.scheduler->delay_microseconds(100);
}
/*Initialise PCM
See BCM2711 documentation:
*/
void RCInput_RPI::init_PCM_BCM2711()
{
pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (700MHz) // https://www.raspberrypi.org/forums/viewtopic.php?t=251902
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI4_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<< 12); // 0x5A001900; //0x0x5A001900; //0x5A000000 | ((50000/RCIN_RPI_SAMPLE_FREQ)<< 4); // Set pcm div. If we need to configure DMA frequency.
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10; //PCM mode 0b0010010000000000
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
hal.scheduler->delay_microseconds(100);

3
libraries/AP_HAL_Linux/RCInput_RPI.h
View File

@ -131,12 +131,15 @@ private:
} rc_channels[RCIN_RPI_CHN_NUM];
bool _initialized = false;
int _version =0;
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_PCM_BCM2835();
void init_PCM_BCM2711();
void init_DMA();
void init_buffer();
static void stop_dma();