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

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170
  1. /*
  2. * Saitek X52 Arduino USB Host Shield Library.
  3. * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
  4. *
  5. * Based on "x52pro-linux" by Nirenjan Krishnan
  6. * https://github.com/nirenjan/x52pro-linux
  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 "x52.h"
  13. #define TIME_24H_FORMAT
  14. #define DEBUG_OUTPUT
  15. X52::X52(USB* u, HID* h) : usb(u), hid(h) { }
  16. void X52::setTime(uint8_t h, uint8_t m) {
  17. uint8_t ret = sendCommand(X52_TIME_CLOCK1, m | ((h & 0x7F) << 8)
  18. #ifdef TIME_24H_FORMAT
  19. | (1 << 15)
  20. #endif
  21. );
  22. if (ret) {
  23. #ifdef DEBUG_OUTPUT
  24. Serial.print("Error setting time: ");
  25. Serial.println(ret, DEC);
  26. #endif
  27. }
  28. }
  29. void X52::setTimeOffset(uint8_t cl, int16_t offset) {
  30. if (offset < -1023) {
  31. offset = -1023;
  32. }
  33. if (offset > 1023) {
  34. offset = 1023;
  35. }
  36. uint8_t negative = 0;
  37. if (offset < 0) {
  38. negative = 1;
  39. offset = -offset;
  40. }
  41. uint8_t ret = sendCommand(cl ? X52_OFFS_CLOCK3 : X52_OFFS_CLOCK2,
  42. (negative << 10) | (offset & 0x03FF)
  43. #ifdef TIME_24H_FORMAT
  44. | (1 << 15)
  45. #endif
  46. );
  47. if (ret) {
  48. #ifdef DEBUG_OUTPUT
  49. Serial.print("Error setting offset: ");
  50. Serial.println(ret, DEC);
  51. #endif
  52. }
  53. }
  54. void X52::setDate(uint8_t dd, uint8_t mm, uint8_t yy) {
  55. uint8_t ret = sendCommand(X52_DATE_DDMM, dd | (mm << 8));
  56. if (!ret) {
  57. ret = sendCommand(X52_DATE_YEAR, yy);
  58. }
  59. if (ret) {
  60. #ifdef DEBUG_OUTPUT
  61. Serial.print("Error setting date: ");
  62. Serial.println(ret, DEC);
  63. #endif
  64. }
  65. }
  66. void X52::setBlink(uint8_t state) {
  67. uint8_t ret = sendCommand(X52_BLINK_INDICATOR, state ? X52_BLINK_ON : X52_BLINK_OFF);
  68. if (ret != 0) {
  69. #ifdef DEBUG_OUTPUT
  70. Serial.print("Error setting blink: ");
  71. Serial.println(ret, DEC);
  72. #endif
  73. }
  74. }
  75. void X52::setShift(uint8_t state) {
  76. uint8_t ret = sendCommand(X52_SHIFT_INDICATOR, state ? X52_SHIFT_ON : X52_SHIFT_OFF);
  77. if (ret != 0) {
  78. #ifdef DEBUG_OUTPUT
  79. Serial.print("Error setting shift: ");
  80. Serial.println(ret, DEC);
  81. #endif
  82. }
  83. }
  84. void X52::setMFDText(uint8_t line, const char* text) {
  85. const static uint16_t lines[3] = {
  86. X52_MFD_LINE1,
  87. X52_MFD_LINE2,
  88. X52_MFD_LINE3
  89. };
  90. if (line >= 3) {
  91. return;
  92. }
  93. uint8_t ret = sendCommand(X52_MFD_CLEAR_LINE | lines[line], 0);
  94. if (ret) {
  95. #ifdef DEBUG_OUTPUT
  96. Serial.print("Error clearing line: ");
  97. Serial.println(ret, DEC);
  98. #endif
  99. return;
  100. }
  101. for (uint8_t i = 0; i < 16; i += 2) {
  102. if (text[i] == '\0') {
  103. break;
  104. }
  105. uint16_t value = text[i];
  106. if (text[i + 1] != '\0') {
  107. value |= text[i + 1] << 8;
  108. }
  109. ret = sendCommand(X52_MFD_WRITE_LINE | lines[line], value);
  110. if (ret) {
  111. #ifdef DEBUG_OUTPUT
  112. Serial.print("Error writing to line: ");
  113. Serial.println(ret, DEC);
  114. #endif
  115. return;
  116. }
  117. if (text[i + 1] == '\0') {
  118. break;
  119. }
  120. }
  121. }
  122. uint8_t X52::sendCommand(uint16_t command, uint16_t val) {
  123. if ((!usb) || (!hid)) {
  124. #ifdef DEBUG_OUTPUT
  125. Serial.println("Invalid objects!");
  126. #endif
  127. return 42;
  128. }
  129. const uint8_t valLo = (val & 0x00FF);
  130. const uint8_t valHi = (val & 0xFF00) >> 8;
  131. #ifdef DEBUG_OUTPUT
  132. Serial.println("Sending X52 Ctrl-Req...");
  133. #endif
  134. uint8_t ret = usb->ctrlReq(hid->GetAddress(), 0,
  135. USB_SETUP_TYPE_VENDOR | USB_SETUP_RECIPIENT_DEVICE,
  136. X52_VENDOR_REQUEST, valLo, valHi, command,
  137. 0, 0, NULL, NULL);
  138. if (ret != 0) {
  139. #ifdef DEBUG_OUTPUT
  140. Serial.print("Ctrl-Req Error Code: ");
  141. Serial.println(val, DEC);
  142. #endif
  143. }
  144. return ret;
  145. }