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

HAL_ChibiOS: use 2 channels in SoftSigReaderInt.cpp

Browse Source
This commit is contained in:
Alexander Malishev 2018-06-02 17:19:46 +04:00 committed by Andrew Tridgell
parent fd45b3a69b
commit eac5d13f1c
5 changed files with 93 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
libraries/AP_HAL_ChibiOS/SoftSigReaderInt.cpp
View File

@ -24,6 +24,10 @@ extern const AP_HAL::HAL& hal;
#if HAL_USE_EICU == TRUE
#if STM32_EICU_USE_TIM10 || STM32_EICU_USE_TIM11 || STM32_EICU_USE_TIM13 || STM32_EICU_USE_TIM14
#error "Timers with only one channel are not supported"
#endif
// singleton instance
SoftSigReaderInt *SoftSigReaderInt::_instance;
@ -32,68 +36,79 @@ SoftSigReaderInt::SoftSigReaderInt()
_instance = this;
}
eicuchannel_t SoftSigReaderInt::get_pair_channel(eicuchannel_t channel)
{
switch (channel) {
case EICU_CHANNEL_1:
return EICU_CHANNEL_2;
case EICU_CHANNEL_2:
return EICU_CHANNEL_1;
case EICU_CHANNEL_3:
return EICU_CHANNEL_4;
case EICU_CHANNEL_4:
return EICU_CHANNEL_3;
case EICU_CHANNEL_ENUM_END:
return EICU_CHANNEL_ENUM_END;
}
return EICU_CHANNEL_ENUM_END;
}
void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
{
last_value = 0;
_icu_drv = icu_drv;
eicuchannel_t aux_chan = get_pair_channel(chan);
icucfg.dier = 0;
icucfg.frequency = INPUT_CAPTURE_FREQUENCY;
for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) {
icucfg.iccfgp[i]=nullptr;
}
//configure main channel
icucfg.iccfgp[chan] = &channel_config;
#ifdef HAL_RCIN_IS_INVERTED
channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
#else
channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
#endif
channel_config.capture_cb = _irq_handler;
channel_config.capture_cb = nullptr;
//configure aux channel
icucfg.iccfgp[aux_chan] = &aux_channel_config;
#ifdef HAL_RCIN_IS_INVERTED
aux_channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
#else
aux_channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
#endif
aux_channel_config.capture_cb = _irq_handler;
eicuStart(_icu_drv, &icucfg);
//sets input filtering to 4 timer clock
stm32_timer_set_input_filter(_icu_drv->tim, chan, 2);
//sets input for aux_chan
stm32_timer_set_channel_input(_icu_drv->tim, aux_chan, 2);
eicuEnable(_icu_drv);
}
inline void invert_polarity(EICUDriver *eicup, eicuchannel_t channel)
void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel)
{
eicup->tim->CCER ^= STM32_TIM_CCER_CC1P << (channel * 4);
}
//check for CCxOF, if it was set by timer hw, it means we missed 2 or more transitions
inline bool overcapture_occured(EICUDriver *eicup, eicuchannel_t channel)
{
bool result = (eicup->tim->SR & (STM32_TIM_SR_CC1OF << channel)) != 0;
eicup->tim->SR &= ~(STM32_TIM_SR_CC1OF << channel);
return result;
}
//check for right pin polarity, if it is wrong it means we missed 1, 3, 5 or more transitions
inline bool wrong_polarity(EICUDriver *eicup, eicuchannel_t channel)
{
bool pin_high = palReadLine(RCININT_PIN);
bool waiting_rising = (eicup->tim->CCER & (STM32_TIM_CCER_CC1P << (channel * 4))) == 0;
if (pin_high && waiting_rising) {
return true;
}
if (!pin_high && !waiting_rising) {
return true;
}
return false;
}
void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t channel)
{
uint16_t value = eicup->tim->CCR[channel];
invert_polarity(eicup, channel);
_instance->sigbuf.push(value);
eicuchannel_t channel = get_pair_channel(aux_channel);
uint16_t value1 = eicup->tim->CCR[channel];
uint16_t value2 = eicup->tim->CCR[aux_channel];
_instance->sigbuf.push(value1);
_instance->sigbuf.push(value2);
//check for missed interrupt
if (overcapture_occured(eicup, channel) || wrong_polarity(eicup, channel)) {
uint32_t mask = (STM32_TIM_SR_CC1OF << channel) | (STM32_TIM_SR_CC1OF << aux_channel);
if ((eicup->tim->SR & mask) != 0) {
//we have missed some pulses
//try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)
_instance->sigbuf.push(value);
//try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)
_instance->sigbuf.push(value2);
//second 0 width pulse to keep polarity right
_instance->sigbuf.push(value);
_instance->sigbuf.push(value2);
//reset overcapture mask
eicup->tim->SR &= ~mask;
}
}
@ -101,7 +116,7 @@ bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1)
{
uint16_t p0, p1;
if (sigbuf.available() >= 2) {
if (sigbuf.pop(p0)&&sigbuf.pop(p1)) {
if (sigbuf.pop(p0) && sigbuf.pop(p1)) {
widths0 = uint16_t(p0 - last_value);
widths1 = uint16_t(p1 - p0);
last_value = p1;

3
libraries/AP_HAL_ChibiOS/SoftSigReaderInt.h
View File

@ -45,10 +45,13 @@ private:
static SoftSigReaderInt *_instance;
static void _irq_handler(EICUDriver *eicup, eicuchannel_t channel);
static eicuchannel_t get_pair_channel(eicuchannel_t channel);
ObjectBuffer<uint16_t> sigbuf{MAX_SIGNAL_TRANSITIONS};
EICUConfig icucfg;
EICUChannelConfig channel_config;
EICUChannelConfig aux_channel_config;
EICUDriver* _icu_drv = nullptr;
uint16_t last_value;
};

36
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
View File

@ -19,6 +19,9 @@
#include <string.h>
#include <stm32_dma.h>
/*
setup the timer capture digital filter for a channel
*/
void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode)
{
switch (channel) {
@ -37,6 +40,39 @@ void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t fil
}
}
/*
set the input source of a timer channel
*/
void stm32_timer_set_channel_input(stm32_tim_t *tim, uint8_t channel, uint8_t input_source)
{
switch (channel) {
case 0:
tim->CCER &= ~STM32_TIM_CCER_CC1E;
tim->CCMR1 &= ~STM32_TIM_CCMR1_CC1S_MASK;
tim->CCMR1 |= STM32_TIM_CCMR1_CC1S(input_source);
tim->CCER |= STM32_TIM_CCER_CC1E;
break;
case 1:
tim->CCER &= ~STM32_TIM_CCER_CC2E;
tim->CCMR1 &= ~STM32_TIM_CCMR1_CC2S_MASK;
tim->CCMR1 |= STM32_TIM_CCMR1_CC2S(input_source);
tim->CCER |= STM32_TIM_CCER_CC2E;
break;
case 2:
tim->CCER &= ~STM32_TIM_CCER_CC3E;
tim->CCMR2 &= ~STM32_TIM_CCMR2_CC3S_MASK;
tim->CCMR2 |= STM32_TIM_CCMR2_CC3S(input_source);
tim->CCER |= STM32_TIM_CCER_CC3E;
break;
case 3:
tim->CCER &= ~STM32_TIM_CCER_CC4E;
tim->CCMR2 &= ~STM32_TIM_CCMR2_CC4S_MASK;
tim->CCMR2 |= STM32_TIM_CCMR2_CC4S(input_source);
tim->CCER |= STM32_TIM_CCER_CC4E;
break;
}
}
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
void show_stack_usage(void)
{

3
libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
View File

@ -21,6 +21,7 @@ extern "C" {
#endif
void stm32_timer_set_input_filter(stm32_tim_t *tim, uint8_t channel, uint8_t filter_mode);
void stm32_timer_set_channel_input(stm32_tim_t *tim, uint8_t channel, uint8_t input_source);
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
// print stack usage
@ -32,7 +33,7 @@ size_t mem_available(void);
void *malloc_ccm(size_t size);
void *malloc_dtcm(size_t size);
void *malloc_dma(size_t size);
#ifdef __cplusplus
}
#endif

1
libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
View File

@ -701,7 +701,6 @@ def write_PWM_config(f):
f.write('#define STM32_EICU_USE_TIM%u TRUE\n' % n)
f.write('#define RCININT_EICU_TIMER EICUD%u\n' % n)
f.write('#define RCININT_EICU_CHANNEL EICU_CHANNEL_%u\n' % int(chan_str))
f.write('#define RCININT_PIN HAL_GPIO_PIN_TIM%s_CH%s\n' % (timer_str, chan_str))
f.write('\n')
if alarm is not None: