You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
131 lines
3.8 KiB
131 lines
3.8 KiB
/* |
|
* This file is free software: you can redistribute it and/or modify it |
|
* under the terms of the GNU General Public License as published by the |
|
* Free Software Foundation, either version 3 of the License, or |
|
* (at your option) any later version. |
|
* |
|
* This file is distributed in the hope that it will be useful, but |
|
* WITHOUT ANY WARRANTY; without even the implied warranty of |
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
|
* See the GNU General Public License for more details. |
|
* |
|
* You should have received a copy of the GNU General Public License along |
|
* with this program. If not, see <http://www.gnu.org/licenses/>. |
|
* |
|
*/ |
|
|
|
#include "SoftSigReaderInt.h" |
|
#include "hwdef/common/stm32_util.h" |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS |
|
|
|
using namespace ChibiOS; |
|
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::_singleton; |
|
|
|
SoftSigReaderInt::SoftSigReaderInt() |
|
{ |
|
_singleton = 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 = 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); |
|
} |
|
|
|
void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel) |
|
{ |
|
eicuchannel_t channel = get_pair_channel(aux_channel); |
|
pulse_t pulse; |
|
pulse.w0 = eicup->tim->CCR[channel]; |
|
pulse.w1 = eicup->tim->CCR[aux_channel]; |
|
|
|
_singleton->sigbuf.push(pulse); |
|
|
|
//check for missed interrupt |
|
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) |
|
pulse.w0 = 0; |
|
pulse.w1 = 0; |
|
_singleton->sigbuf.push(pulse); |
|
//reset overcapture mask |
|
eicup->tim->SR &= ~mask; |
|
} |
|
} |
|
|
|
bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1) |
|
{ |
|
if (sigbuf.available() >= 2) { |
|
pulse_t pulse; |
|
if (sigbuf.pop(pulse)) { |
|
widths0 = uint16_t(pulse.w0 - last_value); |
|
widths1 = uint16_t(pulse.w1 - pulse.w0); |
|
last_value = pulse.w1; |
|
return true; |
|
} |
|
} |
|
return false; |
|
} |
|
|
|
#endif // HAL_USE_EICU |
|
|
|
#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
|
|