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

events_deadzone.cpp 5.0KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199
  1. /*
  2. * Saitek X52 Arduino USB Host Shield Library.
  3. * Copyright 2016 by Thomas Buck <xythobuz@xythobuz.de>
  4. *
  5. * Based on the USB Host Library HID Joystick example
  6. * https://www.circuitsathome.com/mcu/hid-joystick-code-sample
  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 "data.h"
  13. #include "events.h"
  14. //#define DEBUG_OUTPUT_RAW Serial
  15. //#define DEBUG_OUTPUT Serial
  16. /*
  17. * Deadzone around the axis center, in both directions.
  18. * You can try to counteract a faulty Yaw-Potentiometer
  19. * with this, but it won't be enough in extreme cases.
  20. * X, Y, Z, Rx, Ry, Rz, Slider
  21. */
  22. const GamePadEventData JoystickEventsDeadZone::deadZone(
  23. 4, 4, 2, 2, 5, 25, 2
  24. );
  25. const uint8_t JoystickEventsDeadZone::deadZoneMouseX = 1;
  26. const uint8_t JoystickEventsDeadZone::deadZoneMouseY = 1;
  27. /*
  28. * Absolute values of the axis center. Deadzone will be applied
  29. * in both directions around these values.
  30. * X 11bit, Y 11bit, Z 8bit, Rx 8bit, Ry 8bit, Rz 10bit, Slider 8bit
  31. */
  32. const GamePadEventData JoystickEventsDeadZone::centerValue(
  33. 0x3FF, 0x3FF, 0x7F, 0x7F, 0x7F, 0x1FF, 0x7F
  34. );
  35. const uint8_t JoystickEventsDeadZone::centerMouseX = 0x07;
  36. const uint8_t JoystickEventsDeadZone::centerMouseY = 0x07;
  37. void JoystickEventsDeadZone::OnGamePadChanged(const GamePadEventData& evt) {
  38. #ifdef DEBUG_OUTPUT_RAW
  39. DEBUG_OUTPUT_RAW.print("Raw X: ");
  40. DEBUG_OUTPUT_RAW.print(evt.X, HEX);
  41. DEBUG_OUTPUT_RAW.print(" Y: ");
  42. DEBUG_OUTPUT_RAW.print(evt.Y, HEX);
  43. DEBUG_OUTPUT_RAW.print(" Z: ");
  44. DEBUG_OUTPUT_RAW.print(evt.Z, HEX);
  45. DEBUG_OUTPUT_RAW.print(" Rx: ");
  46. DEBUG_OUTPUT_RAW.print(evt.Rx, HEX);
  47. DEBUG_OUTPUT_RAW.print(" Ry: ");
  48. DEBUG_OUTPUT_RAW.print(evt.Ry, HEX);
  49. DEBUG_OUTPUT_RAW.print(" Rz: ");
  50. DEBUG_OUTPUT_RAW.print(evt.Rz, HEX);
  51. DEBUG_OUTPUT_RAW.print(" S: ");
  52. DEBUG_OUTPUT_RAW.println(evt.Slider, HEX);
  53. #endif
  54. GamePadEventData newData = centerValue;
  55. #ifdef DEBUG_OUTPUT
  56. uint8_t updated = 0;
  57. #endif
  58. if ((evt.X > (centerValue.X + deadZone.X))
  59. || (evt.X < (centerValue.X - deadZone.X))) {
  60. newData.X = evt.X;
  61. #ifdef DEBUG_OUTPUT
  62. updated = 1;
  63. #endif
  64. }
  65. if ((evt.Y > (centerValue.Y + deadZone.Y))
  66. || (evt.Y < (centerValue.Y - deadZone.Y))) {
  67. newData.Y = evt.Y;
  68. #ifdef DEBUG_OUTPUT
  69. updated = 1;
  70. #endif
  71. }
  72. if ((evt.Z > (centerValue.Z + deadZone.Z))
  73. || (evt.Z < (centerValue.Z - deadZone.Z))) {
  74. newData.Z = evt.Z;
  75. #ifdef DEBUG_OUTPUT
  76. updated = 1;
  77. #endif
  78. }
  79. if ((evt.Rx > (centerValue.Rx + deadZone.Rx))
  80. || (evt.Rx < (centerValue.Rx - deadZone.Rx))) {
  81. newData.Rx = evt.Rx;
  82. #ifdef DEBUG_OUTPUT
  83. updated = 1;
  84. #endif
  85. }
  86. if ((evt.Ry > (centerValue.Ry + deadZone.Ry))
  87. || (evt.Ry < (centerValue.Ry - deadZone.Ry))) {
  88. newData.Ry = evt.Ry;
  89. #ifdef DEBUG_OUTPUT
  90. updated = 1;
  91. #endif
  92. }
  93. if ((evt.Rz > (centerValue.Rz + deadZone.Rz))
  94. || (evt.Rz < (centerValue.Rz - deadZone.Rz))) {
  95. newData.Rz = evt.Rz;
  96. #ifdef DEBUG_OUTPUT
  97. updated = 1;
  98. #endif
  99. }
  100. if ((evt.Slider > (centerValue.Slider + deadZone.Slider))
  101. || (evt.Slider < (centerValue.Slider - deadZone.Slider))) {
  102. newData.Slider = evt.Slider;
  103. #ifdef DEBUG_OUTPUT
  104. updated = 1;
  105. #endif
  106. }
  107. #ifdef DEBUG_OUTPUT
  108. if (updated) {
  109. DEBUG_OUTPUT.print("X: ");
  110. DEBUG_OUTPUT.print(newData.X, HEX);
  111. DEBUG_OUTPUT.print(" Y: ");
  112. DEBUG_OUTPUT.print(newData.Y, HEX);
  113. DEBUG_OUTPUT.print(" Z: ");
  114. DEBUG_OUTPUT.print(newData.Z, HEX);
  115. DEBUG_OUTPUT.print(" Rx: ");
  116. DEBUG_OUTPUT.print(newData.Rx, HEX);
  117. DEBUG_OUTPUT.print(" Ry: ");
  118. DEBUG_OUTPUT.print(newData.Ry, HEX);
  119. DEBUG_OUTPUT.print(" Rz: ");
  120. DEBUG_OUTPUT.print(newData.Rz, HEX);
  121. DEBUG_OUTPUT.print(" S: ");
  122. DEBUG_OUTPUT.println(newData.Slider, HEX);
  123. }
  124. #endif
  125. if (client) {
  126. client->OnGamePadChanged(newData);
  127. }
  128. }
  129. void JoystickEventsDeadZone::OnMouseMoved(uint8_t x, uint8_t y) {
  130. #ifdef DEBUG_OUTPUT_RAW
  131. DEBUG_OUTPUT_RAW.print("Raw Mouse X: ");
  132. DEBUG_OUTPUT_RAW.print(x, HEX);
  133. DEBUG_OUTPUT_RAW.print("\tY: ");
  134. DEBUG_OUTPUT_RAW.println(y, HEX);
  135. #endif
  136. uint8_t newX = centerMouseX;
  137. uint8_t newY = centerMouseY;
  138. #ifdef DEBUG_OUTPUT
  139. uint8_t updated = 0;
  140. #endif
  141. if ((x > (centerMouseX + deadZoneMouseX))
  142. || (x < (centerMouseX - deadZoneMouseX))) {
  143. newX = x;
  144. #ifdef DEBUG_OUTPUT
  145. updated = 1;
  146. #endif
  147. }
  148. if ((y > (centerMouseY + deadZoneMouseY))
  149. || (y < (centerMouseY - deadZoneMouseY))) {
  150. newY = y;
  151. #ifdef DEBUG_OUTPUT
  152. updated = 1;
  153. #endif
  154. }
  155. #ifdef DEBUG_OUTPUT
  156. if (updated) {
  157. DEBUG_OUTPUT.print("Mouse X: ");
  158. DEBUG_OUTPUT.print(newX, HEX);
  159. DEBUG_OUTPUT.print("\tY: ");
  160. DEBUG_OUTPUT.println(newY, HEX);
  161. }
  162. #endif
  163. if (client) {
  164. client->OnMouseMoved(x, y);
  165. }
  166. }