HAL_ChibiOS: make SoftSigReader considerably more efficient

this reduces interrupt latency on the F100, which allows us to use all
RC input protocols on the pulse based decoder, allowing for more
flexibility in RC input protocols
This commit is contained in:
Andrew Tridgell 2018-11-03 16:46:02 +11:00
parent 9ea03e085d
commit 5dd0086698
3 changed files with 28 additions and 31 deletions

View File

@ -118,10 +118,11 @@ void RCInput::_timer_tick(void)
return;
}
#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE
uint32_t width_s0, width_s1;
while(sig_reader.read(width_s0, width_s1)) {
rcin_prot.process_pulse(width_s0, width_s1);
const uint32_t *p;
uint32_t n;
while ((p = sig_reader.sigbuf.readptr(n)) != nullptr) {
rcin_prot.process_pulse_list(p, n, sig_reader.need_swap);
sig_reader.sigbuf.advance(n);
}
if (rcin_prot.new_input()) {
@ -133,10 +134,12 @@ void RCInput::_timer_tick(void)
_rc_values[i] = rcin_prot.read(i);
}
rcin_mutex.give();
#ifndef HAL_NO_UARTDRIVER
if (rcin_prot.protocol_name() != last_protocol) {
last_protocol = rcin_prot.protocol_name();
gcs().send_text(MAV_SEVERITY_DEBUG, "RCInput: decoding %s", last_protocol);
}
#endif
}
#endif

View File

@ -30,10 +30,14 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
if (chan > ICU_CHANNEL_2) {
return false;
}
signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*_bounce_buf_size, AP_HAL::Util::MEM_DMA_SAFE);
signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*SOFTSIG_BOUNCE_BUF_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
if (signal == nullptr) {
return false;
}
signal2 = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*SOFTSIG_BOUNCE_BUF_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
if (signal2 == nullptr) {
return false;
}
_icu_drv = icu_drv;
//Setup Burst transfer of period and width measurement
dma = STM32_DMA_STREAM(dma_stream);
@ -53,7 +57,7 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
dmamode |= STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD |
STM32_DMA_CR_MSIZE_WORD | STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE;
dmaStreamSetMemory0(dma, signal);
dmaStreamSetTransactionSize(dma, _bounce_buf_size);
dmaStreamSetTransactionSize(dma, SOFTSIG_BOUNCE_BUF_SIZE);
dmaStreamSetMode(dma, dmamode);
icucfg.frequency = INPUT_CAPTURE_FREQUENCY;
@ -91,14 +95,18 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
void SoftSigReader::_irq_handler(void* self, uint32_t flags)
{
SoftSigReader* sig_reader = (SoftSigReader*)self;
sig_reader->sigbuf.push(sig_reader->signal, sig_reader->_bounce_buf_size);
// we need to restart the DMA as quickly as possible to prevent losing pulses, so we
// make a fixed length copy to a 2nd buffer. On the F100 this reduces the time with DMA
// disabled from 20us to under 1us
memcpy(sig_reader->signal2, sig_reader->signal, SOFTSIG_BOUNCE_BUF_SIZE*4);
//restart the DMA transfers
dmaStreamDisable(sig_reader->dma);
dmaStreamSetPeripheral(sig_reader->dma, &sig_reader->_icu_drv->tim->DMAR);
dmaStreamSetMemory0(sig_reader->dma, sig_reader->signal);
dmaStreamSetTransactionSize(sig_reader->dma, sig_reader->_bounce_buf_size);
dmaStreamSetTransactionSize(sig_reader->dma, SOFTSIG_BOUNCE_BUF_SIZE);
dmaStreamSetMode(sig_reader->dma, sig_reader->dmamode);
dmaStreamEnable(sig_reader->dma);
sig_reader->sigbuf.push(sig_reader->signal2, SOFTSIG_BOUNCE_BUF_SIZE);
}
@ -121,24 +129,6 @@ bool SoftSigReader::read(uint32_t &widths0, uint32_t &widths1)
return true;
}
bool SoftSigReader::set_bounce_buf_size(uint16_t buf_size)
{
if (buf_size > _bounce_buf_size) {
if (signal) {
hal.util->free_type(signal, sizeof(uint32_t)*_bounce_buf_size, AP_HAL::Util::MEM_DMA_SAFE);
}
signal = (uint32_t*)hal.util->malloc_type(sizeof(uint32_t)*buf_size, AP_HAL::Util::MEM_DMA_SAFE);
if (signal == nullptr) {
return false;
}
_bounce_buf_size = buf_size;
} else {
_bounce_buf_size = buf_size;
}
return true;
}
#endif // HAL_USE_ICU
#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

View File

@ -23,29 +23,33 @@
#if HAL_USE_ICU == TRUE
#define INPUT_CAPTURE_FREQUENCY 1000000 //capture unit in microseconds
#ifndef SOFTSIG_MAX_SIGNAL_TRANSITIONS
#define SOFTSIG_MAX_SIGNAL_TRANSITIONS 256
#endif
#define DEFAULT_BOUNCE_BUF_SIZE 32
// we use a small bounce buffer size to minimise time in the DMA
// callback IRQ
#define SOFTSIG_BOUNCE_BUF_SIZE 8
class ChibiOS::SoftSigReader {
friend class ChibiOS::RCInput;
public:
bool attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan, uint8_t dma_stream, uint32_t dma_channel);
bool read(uint32_t &widths0, uint32_t &widths1);
//This sets the size of bounce buffer size, which in turn controls the rate of interrupt from DMA
bool set_bounce_buf_size(uint16_t buf_size);
private:
uint32_t *signal;
uint32_t *signal2;
static void _irq_handler(void* self, uint32_t flags);
uint8_t num_timer_channels;
ObjectBuffer<uint32_t> sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS};
uint8_t enable_chan_mask;
uint8_t max_pulse_width;
const stm32_dma_stream_t* dma;
uint32_t dmamode;
ICUConfig icucfg;
ICUDriver* _icu_drv = nullptr;
uint16_t _bounce_buf_size = DEFAULT_BOUNCE_BUF_SIZE;
ObjectBuffer<uint32_t> sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS};
bool need_swap;
};