Control drones with a proper joystick
Ви не можете вибрати більше 25 тем Теми мають розпочинатися з літери або цифри, можуть містити дефіси (-) і не повинні перевищувати 35 символів.

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100
  1. /*
  2. * Combined-PPM signal generator
  3. *
  4. * The whole CPPM pulse should be 20ms long.
  5. * It contains 8 channels with 2ms each, followed
  6. * by 4ms of silence. One channel pulse varies between
  7. * 1 and 2 ms. They are seperated with a very short low
  8. * pulse of about 0.1ms.
  9. *
  10. * 20.000us
  11. * - 8 * 2.000us = 16.000us
  12. * - 9 * 100us = 900us
  13. * = 3.100us
  14. *
  15. * 1 2 3 4 5 6 7 8
  16. * ___ ___ ___ ___ ___ ___ ___ ___ __________
  17. * | | | | | | | | | | | | | | | | | | |
  18. * | | | | | | | | | | | | | | | | | | |
  19. * | | | | | | | | | | | | | | | | | | |
  20. * |_| |_| |_| |_| |_| |_| |_| |_| |_| |
  21. *
  22. * States:
  23. * 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
  24. *
  25. * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
  26. *
  27. * This program is free software; you can redistribute it and/or
  28. * modify it under the terms of the GNU General Public License as
  29. * published by the Free Software Foundation, version 2.
  30. */
  31. #include <TimerOne.h>
  32. #include "cppm.h"
  33. #define CHANNELS 8
  34. #define MAX_STATES ((2 * CHANNELS) + 1)
  35. #define WHOLE_PULSE_WIDTH 20000
  36. #define PULSE_WIDTH 2000
  37. #define MAX_PULSE_WIDTH (CHANNELS * PULSE_WIDTH) // 16.000
  38. #define PULSE_LOW 100
  39. #define PULSE_LOW_SUM ((CHANNELS + 1) * PULSE_LOW) // 900
  40. #define MIN_WAIT (WHOLE_PULSE_WIDTH - MAX_PULSE_WIDTH - PULSE_LOW_SUM) // 3100
  41. #define TIME_AFTER_OVERFLOW 128
  42. #define TIME_MULTIPLIER 2
  43. volatile uint16_t cppmData[CHANNELS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
  44. volatile uint16_t delaySum = 0;
  45. volatile uint8_t state = 0;
  46. static void triggerIn(uint16_t us);
  47. static void nextState(void);
  48. void cppmInit(void) {
  49. pinMode(CPPM_OUTPUT_PIN, OUTPUT);
  50. digitalWrite(CPPM_OUTPUT_PIN, LOW);
  51. Timer1.initialize();
  52. Timer1.attachInterrupt(&nextState);
  53. state = 0;
  54. delaySum = MIN_WAIT;
  55. triggerIn(PULSE_LOW);
  56. }
  57. void cppmCopy(uint16_t *data) {
  58. cli();
  59. for (int i = 0; i < CHANNELS; i++) {
  60. cppmData[i] = data[i];
  61. }
  62. sei();
  63. }
  64. static void triggerIn(uint16_t us) {
  65. Timer1.setPeriod(us);
  66. // TODO reset timer?
  67. }
  68. static void nextState(void) {
  69. // TODO stop timer?
  70. state++;
  71. if (state > MAX_STATES) {
  72. state = 0;
  73. delaySum = MIN_WAIT;
  74. }
  75. if ((state % 2) == 0) {
  76. // pulse pause
  77. digitalWrite(CPPM_OUTPUT_PIN, LOW);
  78. triggerIn(PULSE_LOW);
  79. } else {
  80. digitalWrite(CPPM_OUTPUT_PIN, HIGH);
  81. if (state <= 15) {
  82. // normal ppm pulse
  83. uint8_t index = state / 2;
  84. triggerIn(cppmData[index]);
  85. delaySum += PULSE_WIDTH - cppmData[index];
  86. } else {
  87. // sync pulse
  88. triggerIn(delaySum);
  89. }
  90. }
  91. }