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

AP_HAL_Linux: support PWM input for BH hat

master
Aaron Wang Shi 9 years ago committed by Lucas De Marchi
parent
f449107563
commit
79d56073f7
4 changed files with 168 additions and 92 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. 36
      libraries/AP_HAL_Linux/RCInput.cpp
  2. 7
      libraries/AP_HAL_Linux/RCInput.h
  3. 182
      libraries/AP_HAL_Linux/RCInput_RPI.cpp
  4. 35
      libraries/AP_HAL_Linux/RCInput_RPI.h

36
libraries/AP_HAL_Linux/RCInput.cpp
Unescape View File

@ -42,6 +42,11 @@ uint8_t RCInput::num_channels() @@ -42,6 +42,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)
{
new_rc_input = false;
@ -312,10 +317,18 @@ reset: @@ -312,10 +317,18 @@ 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: 700 ~ 2300
new_rc_input = true;
}
}
/*
process a RC input pulse of the given width
*/
void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel)
{
#if 0
// useful for debugging
@ -327,14 +340,23 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1) @@ -327,14 +340,23 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1);
}
#endif
// treat as PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
// treat as SBUS
_process_sbus_pulse(width_s0, width_s1);
if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) {
// treat as PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
// treat as SBUS
_process_sbus_pulse(width_s0, width_s1);
// treat as DSM
_process_dsm_pulse(width_s0, width_s1);
// treat as DSM
_process_dsm_pulse(width_s0, width_s1);
} else {
// treat as PWM
_process_pwm_pulse(channel, width_s0, width_s1);
}
}
/*

7
libraries/AP_HAL_Linux/RCInput.h
Unescape View File

@ -5,6 +5,7 @@ @@ -5,6 +5,7 @@
#include "AP_HAL_Linux.h"
#define LINUX_RC_INPUT_NUM_CHANNELS 16
#define LINUX_RC_INPUT_CHANNEL_INVALID (999)
class Linux::RCInput : public AP_HAL::RCInput {
public:
@ -17,6 +18,8 @@ public: @@ -17,6 +18,8 @@ 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);
@ -33,7 +36,8 @@ public: @@ -33,7 +36,8 @@ public:
protected:
void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1);
void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1,
uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID);
void _update_periods(uint16_t *periods, uint8_t len);
private:
@ -45,6 +49,7 @@ public: @@ -45,6 +49,7 @@ public:
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);
/* override state */
uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];

182
libraries/AP_HAL_Linux/RCInput_RPI.cpp
Unescape View File

@ -30,13 +30,29 @@ @@ -30,13 +30,29 @@
#define RCIN_RPI_BUFFER_LENGTH 8
#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
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define PPM_INPUT_RPI RPI_GPIO_5
// Same as the circle_buffer size
// See comments in "init_ctrl_data()" to understand values "2"
#define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2)
#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 PPM_INPUT_RPI RPI_GPIO_4
#endif
#define RCIN_RPI_MAX_SIZE_LINE 50
#define RCIN_RPI_MAX_COUNTER 10432
#define RCIN_RPI_SIG_HIGH 1
#define RCIN_RPI_SIG_LOW 0
static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
RPI_GPIO_4
};
#endif // CONFIG_HAL_BOARD_SUBTYPE
//Memory Addresses
#define RCIN_RPI_RPI1_DMA_BASE 0x20007000
@ -267,26 +283,26 @@ void RCInput_RPI::init_ctrl_data() @@ -267,26 +283,26 @@ 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"
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.
/*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 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.
*/
// 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 (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168
{
//Transfer timer every 56th sample
if(i % 56 == 0) {
//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,
@ -300,16 +316,16 @@ void RCInput_RPI::init_ctrl_data() @@ -300,16 +316,16 @@ void RCInput_RPI::init_ctrl_data()
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,
0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
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),
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)
@ -389,15 +405,9 @@ void RCInput_RPI::set_sigaction() @@ -389,15 +405,9 @@ void RCInput_RPI::set_sigaction()
//Initial setup of variables
RCInput_RPI::RCInput_RPI():
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)
{
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
int version = 2;
@ -406,9 +416,9 @@ RCInput_RPI::RCInput_RPI(): @@ -406,9 +416,9 @@ RCInput_RPI::RCInput_RPI():
#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 "113"
//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);
}
RCInput_RPI::~RCInput_RPI()
@ -432,12 +442,15 @@ void RCInput_RPI::init_registers() @@ -432,12 +442,15 @@ void RCInput_RPI::init_registers()
void RCInput_RPI::init()
{
uint64_t signal_states(0);
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();
@ -450,11 +463,17 @@ void RCInput_RPI::init() @@ -450,11 +463,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);
}
@ -463,6 +482,7 @@ void RCInput_RPI::_timer_tick() @@ -463,6 +482,7 @@ void RCInput_RPI::_timer_tick()
{
int j;
void* x;
uint64_t signal_states(0);
//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]);
@ -480,7 +500,7 @@ void RCInput_RPI::_timer_tick() @@ -480,7 +500,7 @@ void RCInput_RPI::_timer_tick()
}
//Processing ready bytes
for (;counter > 0x40;counter--) {
for (;counter > 0x40;) {
//Is it timer samle?
if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
@ -488,36 +508,50 @@ void RCInput_RPI::_timer_tick() @@ -488,36 +508,50 @@ void RCInput_RPI::_timer_tick()
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;
prev_tick = curr_tick;
switch (state) {
case RCIN_RPI_INITIAL_STATE:
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;
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;
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 (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:
rc_channels[i].state = RCIN_RPI_ZERO_STATE;
break;
case RCIN_RPI_ZERO_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;
}
else {
break;
}
case RCIN_RPI_ONE_STATE:
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 CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
_process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1, i);
#else
_process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1);
#endif // CONFIG_HAL_BOARD_SUBTYPE
break;
}
else{
break;
}
}
else
break;
}
rc_channels[i].last_signal = rc_channels[i].curr_signal;
}
last_signal = curr_signal;
curr_pointer++;
curr_pointer += 8;
counter -= 8;
if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
curr_pointer = 0;
}

35
libraries/AP_HAL_Linux/RCInput_RPI.h
Unescape View File

@ -22,6 +22,11 @@ @@ -22,6 +22,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
enum state_t{
RCIN_RPI_INITIAL_STATE = -1,
@ -98,23 +103,33 @@ private: @@ -98,23 +103,33 @@ 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;
uint32_t counter;
uint16_t width_s0;
uint16_t width_s1;
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)
{}
uint8_t curr_signal;
uint8_t last_signal;
state_t state;
AP_HAL::DigitalSource *enable_pin;
uint64_t prev_tick;
uint64_t delta_time;
uint16_t width_s0;
uint16_t width_s1;
uint8_t curr_signal;
uint8_t last_signal;
state_t state;
AP_HAL::DigitalSource *enable_pin;
} rc_channels[RCIN_RPI_CHN_NUM];
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