Control drones with a proper joystick
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.

frsky.cpp 4.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174
  1. /*
  2. * FrSky Telemetry Protocol Host implementation.
  3. * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
  4. *
  5. * Based on the FrSky Telemetry Protocol documentation:
  6. * http://www.frsky-rc.com/download/down.php?id=128
  7. *
  8. * This program is free software; you can redistribute it and/or
  9. * modify it under the terms of the GNU General Public License as
  10. * published by the Free Software Foundation, version 2.
  11. */
  12. #include "frsky.h"
  13. //#define DEBUG_OUTPUT Serial
  14. FrSky::FrSky(Stream* s) : serial(s), dataHandler(0),
  15. alarmThresholdHandler(0), userDataHandler(0), bufferIndex(0) {
  16. for (uint8_t i = 0; i < userDataSize; i++) {
  17. userData[i] = 0;
  18. }
  19. for (uint8_t i = 0; i < bufferSize; i++) {
  20. buffer[i] = 0;
  21. }
  22. }
  23. void FrSky::poll() {
  24. if ((!serial) || (!serial->available())) {
  25. return;
  26. }
  27. uint8_t c = serial->read();
  28. if (c == delimiter) {
  29. #ifdef DEBUG_OUTPUT
  30. DEBUG_OUTPUT.print("Got delimiter at ");
  31. DEBUG_OUTPUT.println(bufferIndex);
  32. #endif
  33. if (bufferIndex < (minPacketSize - 1)) {
  34. #ifdef DEBUG_OUTPUT
  35. DEBUG_OUTPUT.print("Reset to 0: ");
  36. DEBUG_OUTPUT.print(bufferIndex);
  37. DEBUG_OUTPUT.print(" ! <= ");
  38. DEBUG_OUTPUT.println(minPacketSize - 1);
  39. #endif
  40. bufferIndex = 0;
  41. }
  42. if (bufferIndex >= bufferSize) {
  43. #ifdef DEBUG_OUTPUT
  44. DEBUG_OUTPUT.print("too large: ");
  45. DEBUG_OUTPUT.print(bufferIndex);
  46. DEBUG_OUTPUT.print(" / ");
  47. DEBUG_OUTPUT.println(bufferSize);
  48. #endif
  49. bufferIndex = bufferSize - 1;
  50. }
  51. buffer[bufferIndex++] = c;
  52. if (bufferIndex >= minPacketSize) {
  53. #ifdef DEBUG_OUTPUT
  54. DEBUG_OUTPUT.println("Handling...");
  55. #endif
  56. handleMessage();
  57. bufferIndex = 0;
  58. }
  59. } else if ((bufferIndex > 0) && (bufferIndex < bufferSize)) {
  60. buffer[bufferIndex++] = c;
  61. #ifdef DEBUG_OUTPUT
  62. DEBUG_OUTPUT.print("Got ");
  63. DEBUG_OUTPUT.print(c);
  64. DEBUG_OUTPUT.print(" at ");
  65. DEBUG_OUTPUT.println(bufferIndex - 1);
  66. #endif
  67. } else {
  68. #ifdef DEBUG_OUTPUT
  69. DEBUG_OUTPUT.print("Invalid: ");
  70. DEBUG_OUTPUT.print(bufferIndex);
  71. DEBUG_OUTPUT.print(" / ");
  72. DEBUG_OUTPUT.println(bufferSize);
  73. #endif
  74. }
  75. }
  76. void FrSky::pollAlarms() {
  77. serial->write(delimiter);
  78. writeEscaped(idGetAlarms);
  79. for (uint8_t i = 0; i < 8; i++) {
  80. writeEscaped(0);
  81. }
  82. serial->write(delimiter);
  83. }
  84. void FrSky::setAlarm(AlarmThreshold alarm) {
  85. uint8_t id = (alarm.id == analog1_1) ? idAlarm0
  86. : ((alarm.id == analog1_2) ? idAlarm1
  87. : ((alarm.id == analog2_1) ? idAlarm2 : idAlarm3));
  88. serial->write(delimiter);
  89. writeEscaped(id);
  90. writeEscaped(alarm.value);
  91. writeEscaped(alarm.dir);
  92. writeEscaped(alarm.level);
  93. for (uint8_t i = 0; i < 5; i++) {
  94. writeEscaped(0);
  95. }
  96. serial->write(delimiter);
  97. }
  98. void FrSky::writeEscaped(uint8_t v) {
  99. if ((v == delimiter) || (v == escape)) {
  100. v ^= key;
  101. serial->write(escape);
  102. }
  103. serial->write(v);
  104. }
  105. void FrSky::handleMessage() {
  106. #ifdef DEBUG_OUTPUT
  107. DEBUG_OUTPUT.println("FrSky::handleMessage()");
  108. for (uint8_t i = 0; i < bufferIndex; i++) {
  109. DEBUG_OUTPUT.print(buffer[i], HEX);
  110. DEBUG_OUTPUT.print(" ");
  111. }
  112. DEBUG_OUTPUT.println();
  113. #endif
  114. if ((buffer[0] != delimiter) || (buffer[bufferIndex - 1] != delimiter)) {
  115. #ifdef DEBUG_OUTPUT
  116. DEBUG_OUTPUT.println("invalid packet begin/end!");
  117. #endif
  118. return;
  119. }
  120. // Fix escaped bytes
  121. for (uint8_t i = 0; i < (bufferIndex - 1); i++) {
  122. if (buffer[i] == escape) {
  123. buffer[i] = buffer[i + 1] ^ key;
  124. for (uint8_t j = i + 1; j < (bufferIndex - 1); j++) {
  125. buffer[j] = buffer[j + 1];
  126. }
  127. bufferIndex--;
  128. }
  129. }
  130. if (buffer[1] == idVoltageQuality) {
  131. if (dataHandler) {
  132. dataHandler(buffer[2], buffer[3], buffer[4], buffer[5]);
  133. }
  134. } else if (buffer[1] == idUserData) {
  135. uint8_t len = buffer[2];
  136. if (len > userDataSize) {
  137. len = userDataSize;
  138. }
  139. for (uint8_t i = 0; i < len; i++) {
  140. userData[i] = buffer[i + 4];
  141. }
  142. if ((len > 0) && (userDataHandler)) {
  143. userDataHandler(userData, len);
  144. }
  145. } else if ((buffer[1] == idAlarm0) || (buffer[1] == idAlarm1)
  146. || (buffer[1] == idAlarm2) || (buffer[1] == idAlarm3)) {
  147. AnalogValue v = (buffer[1] == idAlarm0) ? analog1_1
  148. : ((buffer[1] == idAlarm1) ? analog1_2
  149. : ((buffer[1] == idAlarm2) ? analog2_1 : analog2_2));
  150. if (alarmThresholdHandler) {
  151. alarmThresholdHandler(AlarmThreshold(v, (GreaterLessThan)buffer[3],
  152. (AlarmLevel)buffer[4], buffer[2]));
  153. }
  154. } else {
  155. #ifdef DEBUG_OUTPUT
  156. DEBUG_OUTPUT.print("Unexpected ID: ");
  157. DEBUG_OUTPUT.println(buffer[1], HEX);
  158. #endif
  159. }
  160. }