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

AP_HAL_Linux: support PWM input for BH hat

Browse Source 
Support 8 PWM channels as RCInput.
This commit is contained in:
Aaron Wang Shi 2015-11-28 17:38:56 +08:00 committed by Andrew Tridgell
parent 800c142ac0
commit 8cca0beba9
4 changed files with 172 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_HAL_Linux/RCInput.cpp
View File

@ -42,6 +42,11 @@ uint8_t RCInput::num_channels()
return _num_channels; return _num_channels;
} }
void RCInput::set_num_channels(uint8_t num)
{
_num_channels = num;
}
uint16_t RCInput::read(uint8_t ch) uint16_t RCInput::read(uint8_t ch)
{ {
new_rc_input = false; new_rc_input = false;
@ -312,10 +317,18 @@ reset:
memset(&dsm_state, 0, sizeof(dsm_state)); 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 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 #if 0
// useful for debugging // useful for debugging
@ -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); fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1);
} }
#endif #endif
// treat as PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
// treat as SBUS if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) {
_process_sbus_pulse(width_s0, width_s1);
// treat as DSM // treat as PPM-sum
_process_dsm_pulse(width_s0, width_s1); _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);
} else {
// treat as PWM
_process_pwm_pulse(channel, width_s0, width_s1);
}
} }
/* /*

7
libraries/AP_HAL_Linux/RCInput.h
View File

@ -5,6 +5,7 @@
#include "AP_HAL_Linux.h" #include "AP_HAL_Linux.h"
#define LINUX_RC_INPUT_NUM_CHANNELS 16 #define LINUX_RC_INPUT_NUM_CHANNELS 16
#define LINUX_RC_INPUT_CHANNEL_INVALID (999)
class Linux::RCInput : public AP_HAL::RCInput { class Linux::RCInput : public AP_HAL::RCInput {
public: public:
@ -17,6 +18,8 @@ public:
virtual void init(); virtual void init();
bool new_input(); bool new_input();
uint8_t num_channels(); uint8_t num_channels();
void set_num_channels(uint8_t num);
uint16_t read(uint8_t ch); uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len); uint8_t read(uint16_t* periods, uint8_t len);
@ -29,7 +32,8 @@ public:
virtual void _timer_tick() {} virtual void _timer_tick() {}
protected: 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); void _update_periods(uint16_t *periods, uint8_t len);
private: private:
@ -41,6 +45,7 @@ public:
void _process_ppmsum_pulse(uint16_t width); void _process_ppmsum_pulse(uint16_t width);
void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1); 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_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 */ /* override state */
uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS]; uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];

184
libraries/AP_HAL_Linux/RCInput_RPI.cpp
View File

@ -1,6 +1,8 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#include "GPIO.h" #include "GPIO.h"
#include "RCInput_RPI.h" #include "RCInput_RPI.h"
#include "Util_RPI.h" #include "Util_RPI.h"
@ -27,10 +29,30 @@
#define RCIN_RPI_BUFFER_LENGTH 8 #define RCIN_RPI_BUFFER_LENGTH 8
#define RCIN_RPI_SAMPLE_FREQ 500 #define RCIN_RPI_SAMPLE_FREQ 500
#define RCIN_RPI_DMA_CHANNEL 0 #define RCIN_RPI_DMA_CHANNEL 0
#define RCIN_RPI_MAX_COUNTER 1300
#define PPM_INPUT_RPI RPI_GPIO_4
#define RCIN_RPI_MAX_SIZE_LINE 50 #define RCIN_RPI_MAX_SIZE_LINE 50
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
// 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 RCIN_RPI_MAX_COUNTER 1304
#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 //Memory Addresses
#define RCIN_RPI_RPI1_DMA_BASE 0x20007000 #define RCIN_RPI_RPI1_DMA_BASE 0x20007000
#define RCIN_RPI_RPI1_CLK_BASE 0x20101000 #define RCIN_RPI_RPI1_CLK_BASE 0x20101000
@ -260,26 +282,26 @@ void RCInput_RPI::init_ctrl_data()
phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
//Init dma control blocks. //Init dma control blocks.
/*We are transferring 1 byte of GPIO register. Every 56th iteration we are /*We are transferring 8 bytes of GPIO register. Every 7th iteration we are
sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need
56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block" 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 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 "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 vitual pages for control blocks): for every 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO
sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so, sampling, 7 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. 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 113 pages we will have (113 * 4096 / 32) = 113 * 128 control Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control
blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO
and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer. and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer.
*/ */
// fprintf(stderr, "ERROR SEARCH1\n"); // fprintf(stderr, "ERROR SEARCH1\n");
uint32_t i = 0; 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 //Transfer timer every 7th sample
if(i % 56 == 0) { if(i % 7 == 0) {
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); 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, 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,
@ -293,16 +315,16 @@ void RCInput_RPI::init_ctrl_data()
cbp += sizeof(dma_cb_t); cbp += sizeof(dma_cb_t);
} }
// Transfer GPIO (1 byte) // Transfer GPIO (8 byte)
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); 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, 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), (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
1, 8,
0, 0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t) ) ); cbp + sizeof(dma_cb_t) ) );
dest += 1; dest += 8;
cbp += sizeof(dma_cb_t); cbp += sizeof(dma_cb_t);
// Delay (for setting sampling frequency) // Delay (for setting sampling frequency)
@ -382,15 +404,9 @@ void RCInput_RPI::set_sigaction()
//Initial setup of variables //Initial setup of variables
RCInput_RPI::RCInput_RPI(): RCInput_RPI::RCInput_RPI():
prev_tick(0),
delta_time(0),
curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ),
curr_pointer(0), curr_pointer(0),
curr_channel(0), curr_channel(0)
width_s0(0),
curr_signal(0),
last_signal(228),
state(RCIN_RPI_INITIAL_STATE)
{ {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
int version = 2; int version = 2;
@ -399,9 +415,9 @@ RCInput_RPI::RCInput_RPI():
#endif #endif
set_physical_addresses(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" //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); 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() RCInput_RPI::~RCInput_RPI()
@ -425,12 +441,15 @@ void RCInput_RPI::init_registers()
void RCInput_RPI::init() void RCInput_RPI::init()
{ {
uint64_t signal_states(0);
init_registers(); init_registers();
//Enable PPM input //Enable PPM or PWM input
enable_pin = hal.gpio->channel(PPM_INPUT_RPI); for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
enable_pin->mode(HAL_GPIO_INPUT); rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]);
rc_channels[i].enable_pin->mode(HAL_GPIO_INPUT);
}
//Configuration //Configuration
set_sigaction(); set_sigaction();
@ -443,11 +462,17 @@ void RCInput_RPI::init()
//Reading first sample //Reading first sample
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
prev_tick = curr_tick;
curr_pointer += 8; curr_pointer += 8;
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));
last_signal = curr_signal; for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
curr_pointer++; 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);
} }
@ -456,6 +481,7 @@ void RCInput_RPI::_timer_tick()
{ {
int j; int j;
void* x; void* x;
uint64_t signal_states(0);
//Now we are getting address in which DMAC is writing at current moment //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]); dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]);
@ -473,7 +499,7 @@ void RCInput_RPI::_timer_tick()
} }
//Processing ready bytes //Processing ready bytes
for (;counter > 0x40;counter--) { for (;counter > 0x40;) {
//Is it timer samle? //Is it timer samle?
if (curr_pointer % (64) == 0) { if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
@ -481,36 +507,50 @@ void RCInput_RPI::_timer_tick()
counter-=8; counter-=8;
} }
//Reading required bit //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));
//If the signal changed for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
if (curr_signal != last_signal) { rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH
delta_time = curr_tick - prev_tick; : RCIN_RPI_SIG_LOW;
prev_tick = curr_tick;
switch (state) { //If the signal changed
case RCIN_RPI_INITIAL_STATE: if (rc_channels[i].curr_signal != rc_channels[i].last_signal) {
state = RCIN_RPI_ZERO_STATE; rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick;
break; rc_channels[i].prev_tick = curr_tick;
case RCIN_RPI_ZERO_STATE: switch (rc_channels[i].state) {
if (curr_signal == 0) { case RCIN_RPI_INITIAL_STATE:
width_s0 = (uint16_t) delta_time; rc_channels[i].state = RCIN_RPI_ZERO_STATE;
state = RCIN_RPI_ONE_STATE; break;
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;
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;
} }
rc_channels[i].last_signal = rc_channels[i].curr_signal;
} }
last_signal = curr_signal; curr_pointer += 8;
curr_pointer++; counter -= 8;
if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
curr_pointer = 0; curr_pointer = 0;
} }

35
libraries/AP_HAL_Linux/RCInput_RPI.h
View File

@ -22,6 +22,11 @@
#include <assert.h> #include <assert.h>
#include <queue> #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{ enum state_t{
RCIN_RPI_INITIAL_STATE = -1, RCIN_RPI_INITIAL_STATE = -1,
@ -98,23 +103,33 @@ private:
Memory_table *con_blocks; Memory_table *con_blocks;
uint64_t curr_tick; uint64_t curr_tick;
uint64_t prev_tick;
uint64_t delta_time;
uint32_t curr_tick_inc; uint32_t curr_tick_inc;
uint32_t curr_pointer; uint32_t curr_pointer;
uint32_t curr_channel; uint32_t curr_channel;
uint32_t counter; uint32_t counter;
uint16_t width_s0; struct RcChannel {
uint16_t width_s1; 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; uint64_t prev_tick;
uint8_t last_signal; uint64_t delta_time;
state_t state; uint16_t width_s0;
uint16_t width_s1;
AP_HAL::DigitalSource *enable_pin;
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 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* map_peripheral(uint32_t base, uint32_t len);