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.
83 lines
2.4 KiB
83 lines
2.4 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 |
|
|
|
#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
|
|
|