SoftSigReader.h 1.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960
  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. * Code by Andrew Tridgell and Siddharth Bharat Purohit
  16. */
  17. #pragma once
  18. #include <AP_HAL/utility/RingBuffer.h>
  19. #include <AP_HAL/AP_HAL_Boards.h>
  20. #include "AP_HAL_ChibiOS.h"
  21. #if HAL_USE_ICU == TRUE
  22. #define INPUT_CAPTURE_FREQUENCY 1000000 //capture unit in microseconds
  23. #ifndef SOFTSIG_MAX_SIGNAL_TRANSITIONS
  24. #define SOFTSIG_MAX_SIGNAL_TRANSITIONS 128
  25. #endif
  26. // we use a small bounce buffer size to minimise time in the DMA
  27. // callback IRQ
  28. #define SOFTSIG_BOUNCE_BUF_SIZE 8
  29. class ChibiOS::SoftSigReader {
  30. friend class ChibiOS::RCInput;
  31. public:
  32. bool attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan, uint8_t dma_stream, uint32_t dma_channel);
  33. private:
  34. uint32_t *signal;
  35. uint32_t signal2[SOFTSIG_BOUNCE_BUF_SIZE];
  36. static void _irq_handler(void* self, uint32_t flags);
  37. uint8_t num_timer_channels;
  38. uint8_t enable_chan_mask;
  39. uint8_t max_pulse_width;
  40. const stm32_dma_stream_t* dma;
  41. uint32_t dmamode;
  42. ICUConfig icucfg;
  43. ICUDriver* _icu_drv = nullptr;
  44. typedef struct PACKED {
  45. uint32_t w0;
  46. uint32_t w1;
  47. } pulse_t;
  48. ObjectBuffer<pulse_t> sigbuf{SOFTSIG_MAX_SIGNAL_TRANSITIONS};
  49. bool need_swap;
  50. };
  51. #endif // HAL_USE_ICU