/* * 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 . * */ #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 #define eicu_lld_invert_polarity(eicup, channel) \ (eicup)->tim->CCER ^= ((uint16_t)(STM32_TIM_CCER_CC1P << ((channel) * 4))) // singleton instance SoftSigReaderInt *SoftSigReaderInt::_instance; SoftSigReaderInt::SoftSigReaderInt() { _instance = this; } void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan) { last_value = 0; _icu_drv = icu_drv; icucfg.dier = 0; icucfg.frequency = INPUT_CAPTURE_FREQUENCY; for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) { icucfg.iccfgp[i]=nullptr; } 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; eicuStart(_icu_drv, &icucfg); //sets input filtering to 4 timer clock stm32_timer_set_input_filter(_icu_drv->tim, chan, 2); eicuEnable(_icu_drv); } void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t channel) { uint16_t value = eicup->tim->CCR[channel]; _instance->sigbuf.push(value); eicu_lld_invert_polarity(eicup, channel); } bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1) { uint16_t p0, p1; if (sigbuf.available() >= 2) { if (sigbuf.pop(p0)&&sigbuf.pop(p1)) { widths0 = uint16_t(p0 - last_value); widths1 = uint16_t(p1 - p0); last_value = p1; return true; } } return false; } #endif // HAL_USE_EICU #endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS