SoftSigReaderInt.cpp 3.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131
  1. /*
  2. * This file is free software: you can redistribute it and/or modify it
  3. * under the terms of the GNU General Public License as published by the
  4. * Free Software Foundation, either version 3 of the License, or
  5. * (at your option) any later version.
  6. *
  7. * This file is distributed in the hope that it will be useful, but
  8. * WITHOUT ANY WARRANTY; without even the implied warranty of
  9. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  10. * See the GNU General Public License for more details.
  11. *
  12. * You should have received a copy of the GNU General Public License along
  13. * with this program. If not, see <http://www.gnu.org/licenses/>.
  14. *
  15. */
  16. #include "SoftSigReaderInt.h"
  17. #include "hwdef/common/stm32_util.h"
  18. #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
  19. using namespace ChibiOS;
  20. extern const AP_HAL::HAL& hal;
  21. #if HAL_USE_EICU == TRUE
  22. #if STM32_EICU_USE_TIM10 || STM32_EICU_USE_TIM11 || STM32_EICU_USE_TIM13 || STM32_EICU_USE_TIM14
  23. #error "Timers with only one channel are not supported"
  24. #endif
  25. // singleton instance
  26. SoftSigReaderInt *SoftSigReaderInt::_singleton;
  27. SoftSigReaderInt::SoftSigReaderInt()
  28. {
  29. _singleton = this;
  30. }
  31. eicuchannel_t SoftSigReaderInt::get_pair_channel(eicuchannel_t channel)
  32. {
  33. switch (channel) {
  34. case EICU_CHANNEL_1:
  35. return EICU_CHANNEL_2;
  36. case EICU_CHANNEL_2:
  37. return EICU_CHANNEL_1;
  38. case EICU_CHANNEL_3:
  39. return EICU_CHANNEL_4;
  40. case EICU_CHANNEL_4:
  41. return EICU_CHANNEL_3;
  42. case EICU_CHANNEL_ENUM_END:
  43. return EICU_CHANNEL_ENUM_END;
  44. }
  45. return EICU_CHANNEL_ENUM_END;
  46. }
  47. void SoftSigReaderInt::init(EICUDriver* icu_drv, eicuchannel_t chan)
  48. {
  49. last_value = 0;
  50. _icu_drv = icu_drv;
  51. eicuchannel_t aux_chan = get_pair_channel(chan);
  52. icucfg.dier = 0;
  53. icucfg.frequency = INPUT_CAPTURE_FREQUENCY;
  54. for (int i=0; i< EICU_CHANNEL_ENUM_END; i++) {
  55. icucfg.iccfgp[i]=nullptr;
  56. }
  57. //configure main channel
  58. icucfg.iccfgp[chan] = &channel_config;
  59. #ifdef HAL_RCIN_IS_INVERTED
  60. channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
  61. #else
  62. channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
  63. #endif
  64. channel_config.capture_cb = nullptr;
  65. //configure aux channel
  66. icucfg.iccfgp[aux_chan] = &aux_channel_config;
  67. #ifdef HAL_RCIN_IS_INVERTED
  68. aux_channel_config.alvl = EICU_INPUT_ACTIVE_LOW;
  69. #else
  70. aux_channel_config.alvl = EICU_INPUT_ACTIVE_HIGH;
  71. #endif
  72. aux_channel_config.capture_cb = _irq_handler;
  73. eicuStart(_icu_drv, &icucfg);
  74. //sets input filtering to 4 timer clock
  75. stm32_timer_set_input_filter(_icu_drv->tim, chan, 2);
  76. //sets input for aux_chan
  77. stm32_timer_set_channel_input(_icu_drv->tim, aux_chan, 2);
  78. eicuEnable(_icu_drv);
  79. }
  80. void SoftSigReaderInt::_irq_handler(EICUDriver *eicup, eicuchannel_t aux_channel)
  81. {
  82. eicuchannel_t channel = get_pair_channel(aux_channel);
  83. pulse_t pulse;
  84. pulse.w0 = eicup->tim->CCR[channel];
  85. pulse.w1 = eicup->tim->CCR[aux_channel];
  86. _singleton->sigbuf.push(pulse);
  87. //check for missed interrupt
  88. uint32_t mask = (STM32_TIM_SR_CC1OF << channel) | (STM32_TIM_SR_CC1OF << aux_channel);
  89. if ((eicup->tim->SR & mask) != 0) {
  90. //we have missed some pulses
  91. //try to reset RCProtocol parser by returning invalid value (i.e. 0 width pulse)
  92. pulse.w0 = 0;
  93. pulse.w1 = 0;
  94. _singleton->sigbuf.push(pulse);
  95. //reset overcapture mask
  96. eicup->tim->SR &= ~mask;
  97. }
  98. }
  99. bool SoftSigReaderInt::read(uint32_t &widths0, uint32_t &widths1)
  100. {
  101. if (sigbuf.available() >= 2) {
  102. pulse_t pulse;
  103. if (sigbuf.pop(pulse)) {
  104. widths0 = uint16_t(pulse.w0 - last_value);
  105. widths1 = uint16_t(pulse.w1 - pulse.w0);
  106. last_value = pulse.w1;
  107. return true;
  108. }
  109. }
  110. return false;
  111. }
  112. #endif // HAL_USE_EICU
  113. #endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS