aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: add invert method and fix width measurement

Browse Source
This commit is contained in:
bugobliterator 2018-01-19 00:43:52 +05:30 committed by Andrew Tridgell
parent f7ac5aa079
commit 02c7513f83
3 changed files with 41 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_HAL_ChibiOS/RCInput.cpp
View File

@ -25,6 +25,7 @@
extern AP_IOMCU iomcu;
#endif
#define SIG_DETECT_TIMEOUT_US 500000
using namespace ChibiOS;
extern const AP_HAL::HAL& hal;
void RCInput::init()
@ -36,6 +37,8 @@ void RCInput::init()
#endif
chMtxObjectInit(&rcin_mutex);
_init = true;
//timeout starts from the initialisation
_rcin_timestamp_last_signal = AP_HAL::micros();
}
bool RCInput::new_input()
@ -183,6 +186,11 @@ void RCInput::_timer_tick(void)
}
chMtxUnlock(&rcin_mutex);
}
//we reset if nothing detected for SIG_DETECT_TIMEOUT_US
if (AP_HAL::micros() - _rcin_timestamp_last_signal > SIG_DETECT_TIMEOUT_US) {
_rcin_timestamp_last_signal = AP_HAL::micros();
sig_reader.invert();
}
#endif
#ifdef HAL_RCINPUT_WITH_AP_RADIO

32
libraries/AP_HAL_ChibiOS/SoftSigReader.cpp
View File

@ -33,6 +33,7 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
if (signal == nullptr) {
return false;
}
_icu_drv = icu_drv;
//Setup Burst transfer of period and width measurement
dma = STM32_DMA_STREAM(dma_stream);
bool dma_allocated = dmaStreamAllocate(dma,
@ -63,15 +64,15 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan,
} else {
icucfg.dier = STM32_TIM_DIER_CC2DE;
}
icuStart(icu_drv, &icucfg);
icuStart(_icu_drv, &icucfg);
//Extended Timer Setup to enable DMA transfer
//selected offset for TIM_CCR1 and for two words
icu_drv->tim->DCR = STM32_TIM_DCR_DBA(0x0D) | STM32_TIM_DCR_DBL(1);
_icu_drv->tim->DCR = STM32_TIM_DCR_DBA(0x0D) | STM32_TIM_DCR_DBL(1);
//Enable DMA
dmaStreamEnable(dma);
//Start Timer
icuStartCapture(icu_drv);
icuStartCapture(_icu_drv);
return true;
}
@ -89,9 +90,15 @@ void SoftSigReader::_irq_handler(void* self, uint32_t flags)
bool SoftSigReader::read(uint32_t &widths0, uint32_t &widths1)
{
if (sigbuf.pop(widths0) && sigbuf.pop(widths1)) {
//the data is period and width, order depending on which channel is used and
//width type (0 or 1) depends on mode being set to HIGH or LOW
if (widths0 > widths1) {
//This happens when ICU_CHANNEL_1 is used
//We need to swap while converting to widths
widths1 = widths0 - widths1;
widths0 -= widths1;
} else {
//This happens when ICU_CHANNEL_2 is used
widths1 -= widths0;
}
} else {
@ -100,6 +107,25 @@ bool SoftSigReader::read(uint32_t &widths0, uint32_t &widths1)
return true;
}
void SoftSigReader::invert()
{
if (_icu_drv == nullptr) {
return;
}
icuStopCapture(_icu_drv);
icuStop(_icu_drv);
if (icucfg.mode == ICU_INPUT_ACTIVE_LOW) {
icucfg.mode = ICU_INPUT_ACTIVE_HIGH;
} else {
icucfg.mode = ICU_INPUT_ACTIVE_LOW;
}
icuStart(_icu_drv, &icucfg);
_icu_drv->tim->DCR = STM32_TIM_DCR_DBA(0x0D) | STM32_TIM_DCR_DBL(1);
icuStartCapture(_icu_drv);
}
bool SoftSigReader::set_bounce_buf_size(uint16_t buf_size)
{
if (buf_size > _bounce_buf_size) {

3
libraries/AP_HAL_ChibiOS/SoftSigReader.h
View File

@ -32,6 +32,8 @@ public:
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);
//inverts the signal input mode
void invert();
private:
uint32_t *signal;
static void _irq_handler(void* self, uint32_t flags);
@ -41,6 +43,7 @@ private:
uint8_t max_pulse_width;
const stm32_dma_stream_t* dma;
ICUConfig icucfg;
ICUDriver* _icu_drv = nullptr;
uint16_t _bounce_buf_size = DEFAULT_BOUNCE_BUF_SIZE;
};