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

AP_HAL_Linux: support PWM input for BH hat

mission-4.1.18
Aaron Wang Shi 8 years ago committed by Lucas De Marchi
parent
8a26ca1ddf
commit
4f2d9c2c4d
4 changed files with 144 additions and 84 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. 13
      libraries/AP_HAL_Linux/RCInput.cpp
  2. 2
      libraries/AP_HAL_Linux/RCInput.h
  3. 154
      libraries/AP_HAL_Linux/RCInput_RPI.cpp
  4. 23
      libraries/AP_HAL_Linux/RCInput_RPI.h

13
libraries/AP_HAL_Linux/RCInput.cpp
Unescape View File

@ -48,6 +48,11 @@ uint8_t RCInput::num_channels() @@ -48,6 +48,11 @@ 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)
{
if (ch >= _num_channels) {
@ -278,6 +283,14 @@ reset: @@ -278,6 +283,14 @@ 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: 700usec ~ 2300usec
rc_input_count++;
}
}
/*
process a RC input pulse of the given width
*/

2
libraries/AP_HAL_Linux/RCInput.h
Unescape View File

@ -19,6 +19,7 @@ public: @@ -19,6 +19,7 @@ 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);
@ -58,6 +59,7 @@ protected: @@ -58,6 +59,7 @@ protected:
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);
// state of ppm decoder
struct {

154
libraries/AP_HAL_Linux/RCInput_RPI.cpp
Unescape View File

@ -32,19 +32,34 @@ @@ -32,19 +32,34 @@
#endif
//Parametres
#define RCIN_RPI_BUFFER_LENGTH 8
#define RCIN_RPI_BUFFER_LENGTH 4
#define RCIN_RPI_SAMPLE_FREQ 500
#define RCIN_RPI_DMA_CHANNEL 0
#define RCIN_RPI_MAX_COUNTER 1300
#define RCIN_RPI_MAX_SIZE_LINE 50
#define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2) // 1 circle_buffer
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define PPM_INPUT_RPI RPI_GPIO_5
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define PPM_INPUT_RPI NAVIO_GPIO_PPM_IN
#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
};
#else
#define RCIN_RPI_SIG_HIGH 1
#define RCIN_RPI_SIG_LOW 0
static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define PAGE_SIZE (4*1024)
NAVIO_GPIO_PPM_IN
#else
#define PPM_INPUT_RPI RPI_GPIO_4
RPI_GPIO_4
#endif
#define RCIN_RPI_MAX_SIZE_LINE 50
};
#endif // CONFIG_HAL_BOARD_SUBTYPE
//Memory Addresses
#define RCIN_RPI_RPI1_DMA_BASE 0x20007000
@ -278,24 +293,24 @@ void RCInput_RPI::init_ctrl_data() @@ -278,24 +293,24 @@ void RCInput_RPI::init_ctrl_data()
phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
// 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"
/* 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 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.
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.
*/
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);
for (uint32_t 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),
@ -303,25 +318,24 @@ void RCInput_RPI::init_ctrl_data() @@ -303,25 +318,24 @@ void RCInput_RPI::init_ctrl_data()
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)
// 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),
1,
8,
0,
(uintptr_t)con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t)));
dest += 1;
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,
/* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 5 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);
@ -413,17 +427,9 @@ void RCInput_RPI::set_sigaction() @@ -413,17 +427,9 @@ void RCInput_RPI::set_sigaction()
// Initial setup of variables
RCInput_RPI::RCInput_RPI():
circle_buffer{nullptr},
con_blocks{nullptr},
prev_tick(0),
delta_time(0),
curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ),
curr_pointer(0),
curr_channel(0),
width_s0(0),
curr_signal(0),
last_signal(228),
state(RCIN_RPI_INITIAL_STATE)
curr_channel(0)
{
}
@ -448,20 +454,26 @@ void RCInput_RPI::init_registers() @@ -448,20 +454,26 @@ void RCInput_RPI::init_registers()
void RCInput_RPI::init()
{
uint64_t signal_states(0);
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
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"
circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version);
con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version);
con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version);
init_registers();
// Enable PPM input
enable_pin = hal.gpio->channel(PPM_INPUT_RPI);
enable_pin->mode(HAL_GPIO_INPUT);
// 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);
}
// Configuration
set_sigaction();
@ -474,11 +486,17 @@ void RCInput_RPI::init() @@ -474,11 +486,17 @@ 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;
curr_signal = *((uint8_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
last_signal = curr_signal;
curr_pointer++;
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);
_initialized = true;
}
@ -487,6 +505,7 @@ void RCInput_RPI::init() @@ -487,6 +505,7 @@ void RCInput_RPI::init()
void RCInput_RPI::_timer_tick()
{
uint32_t counter = 0;
uint64_t signal_states(0);
if (!_initialized) {
return;
@ -494,6 +513,7 @@ void RCInput_RPI::_timer_tick() @@ -494,6 +513,7 @@ 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]);
if (!ad) {
debug("DMA sampling stopped, restarting...\n");
init_ctrl_data();
@ -523,43 +543,53 @@ void RCInput_RPI::_timer_tick() @@ -523,43 +543,53 @@ void RCInput_RPI::_timer_tick()
}
// Processing ready bytes
for (; counter > 0x40; counter--) {
for (;counter > 0x40;) {
// 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;
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 (curr_signal != last_signal) {
delta_time = curr_tick - prev_tick;
prev_tick = curr_tick;
switch (state) {
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:
state = RCIN_RPI_ZERO_STATE;
rc_channels[i].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;
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 (curr_signal == 1) {
width_s1 = (uint16_t)delta_time;
state = RCIN_RPI_ZERO_STATE;
_process_rc_pulse(width_s0, width_s1);
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 (1 == RCIN_RPI_CHN_NUM) {
_process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1);
}
else {
_process_pwm_pulse(i, rc_channels[i].width_s0,
rc_channels[i].width_s1);
}
}
break;
}
}
last_signal = curr_signal;
curr_pointer++;
rc_channels[i].last_signal = rc_channels[i].curr_signal;
}
curr_pointer += 8;
counter -= 8;
if (curr_pointer >= circle_buffer->get_page_count() * PAGE_SIZE) {
curr_pointer = 0;
}
@ -567,4 +597,4 @@ void RCInput_RPI::_timer_tick() @@ -567,4 +597,4 @@ void RCInput_RPI::_timer_tick()
}
}
#endif
#endif // CONFIG_HAL_BOARD_SUBTYPE

23
libraries/AP_HAL_Linux/RCInput_RPI.h
Unescape View File

@ -21,6 +21,11 @@ @@ -21,6 +21,11 @@
#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
namespace Linux {
@ -98,24 +103,34 @@ private: @@ -98,24 +103,34 @@ 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;
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)
{}
uint64_t prev_tick;
uint64_t delta_time;
uint16_t width_s0;
uint16_t width_s1;
uint8_t curr_signal;
uint8_t last_signal;
bool _initialized = false;
state_t state;
AP_HAL::DigitalSource *enable_pin;
} rc_channels[RCIN_RPI_CHN_NUM];
bool _initialized = false;
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