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.

events_deadzone.cpp 4.7KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196
  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 <usb.h>
  13. #include "data.h"
  14. #include "events.h"
  15. //#define DEBUG_OUTPUT_RAW
  16. //#define DEBUG_OUTPUT
  17. // X, Y, Z, Rx, Ry, Rz, Slider
  18. const GamePadEventData JoystickEventsDeadZone::deadZone(
  19. 4, 4, 2, 2, 5, 25, 2
  20. );
  21. const uint8_t JoystickEventsDeadZone::deadZoneMouseX = 1;
  22. const uint8_t JoystickEventsDeadZone::deadZoneMouseY = 1;
  23. // X, Y, Z, Rx, Ry, Rz, Slider
  24. const GamePadEventData JoystickEventsDeadZone::centerValue(
  25. 0x3FF, 0x3FF, 0x7F, 0x7F, 0x7F, 0x1FF, 0x7F
  26. );
  27. const uint8_t JoystickEventsDeadZone::centerMouseX = 0x07;
  28. const uint8_t JoystickEventsDeadZone::centerMouseY = 0x07;
  29. void JoystickEventsDeadZone::OnGamePadChanged(const GamePadEventData& evt) {
  30. #ifdef DEBUG_OUTPUT_RAW
  31. Serial.print("X: ");
  32. PrintHex<uint16_t > (evt.X, 0x80);
  33. Serial.print(" Y: ");
  34. PrintHex<uint16_t > (evt.Y, 0x80);
  35. Serial.print(" Z: ");
  36. PrintHex<uint8_t > (evt.Z, 0x80);
  37. Serial.print(" Rx: ");
  38. PrintHex<uint8_t > (evt.Rx, 0x80);
  39. Serial.print(" Ry: ");
  40. PrintHex<uint8_t > (evt.Ry, 0x80);
  41. Serial.print(" Rz: ");
  42. PrintHex<uint16_t > (evt.Rz, 0x80);
  43. Serial.print(" S: ");
  44. PrintHex<uint8_t > (evt.Slider, 0x80);
  45. Serial.println("");
  46. #endif
  47. GamePadEventData newData = centerValue;
  48. #ifdef DEBUG_OUTPUT
  49. uint8_t updated = 0;
  50. #endif
  51. if ((evt.X > (centerValue.X + deadZone.X))
  52. || (evt.X < (centerValue.X - deadZone.X))) {
  53. newData.X = evt.X;
  54. #ifdef DEBUG_OUTPUT
  55. updated = 1;
  56. #endif
  57. }
  58. if ((evt.Y > (centerValue.Y + deadZone.Y))
  59. || (evt.Y < (centerValue.Y - deadZone.Y))) {
  60. newData.Y = evt.Y;
  61. #ifdef DEBUG_OUTPUT
  62. updated = 1;
  63. #endif
  64. }
  65. if ((evt.Z > (centerValue.Z + deadZone.Z))
  66. || (evt.Z < (centerValue.Z - deadZone.Z))) {
  67. newData.Z = evt.Z;
  68. #ifdef DEBUG_OUTPUT
  69. updated = 1;
  70. #endif
  71. }
  72. if ((evt.Rx > (centerValue.Rx + deadZone.Rx))
  73. || (evt.Rx < (centerValue.Rx - deadZone.Rx))) {
  74. newData.Rx = evt.Rx;
  75. #ifdef DEBUG_OUTPUT
  76. updated = 1;
  77. #endif
  78. }
  79. if ((evt.Ry > (centerValue.Ry + deadZone.Ry))
  80. || (evt.Ry < (centerValue.Ry - deadZone.Ry))) {
  81. newData.Ry = evt.Ry;
  82. #ifdef DEBUG_OUTPUT
  83. updated = 1;
  84. #endif
  85. }
  86. if ((evt.Rz > (centerValue.Rz + deadZone.Rz))
  87. || (evt.Rz < (centerValue.Rz - deadZone.Rz))) {
  88. newData.Rz = evt.Rz;
  89. #ifdef DEBUG_OUTPUT
  90. updated = 1;
  91. #endif
  92. }
  93. if ((evt.Slider > (centerValue.Slider + deadZone.Slider))
  94. || (evt.Slider < (centerValue.Slider - deadZone.Slider))) {
  95. newData.Slider = evt.Slider;
  96. #ifdef DEBUG_OUTPUT
  97. updated = 1;
  98. #endif
  99. }
  100. #ifdef DEBUG_OUTPUT
  101. if (updated) {
  102. Serial.print("X: ");
  103. PrintHex<uint16_t > (newData.X, 0x80);
  104. Serial.print(" Y: ");
  105. PrintHex<uint16_t > (newData.Y, 0x80);
  106. Serial.print(" Z: ");
  107. PrintHex<uint8_t > (newData.Z, 0x80);
  108. Serial.print(" Rx: ");
  109. PrintHex<uint8_t > (newData.Rx, 0x80);
  110. Serial.print(" Ry: ");
  111. PrintHex<uint8_t > (newData.Ry, 0x80);
  112. Serial.print(" Rz: ");
  113. PrintHex<uint16_t > (newData.Rz, 0x80);
  114. Serial.print(" S: ");
  115. PrintHex<uint8_t > (newData.Slider, 0x80);
  116. Serial.println("");
  117. }
  118. #endif
  119. if (client) {
  120. client->OnGamePadChanged(newData);
  121. }
  122. }
  123. void JoystickEventsDeadZone::OnMouseMoved(uint8_t x, uint8_t y) {
  124. #ifdef DEBUG_OUTPUT_RAW
  125. Serial.print("Mouse X: ");
  126. PrintHex<uint8_t >(x, 0x80);
  127. Serial.print("\tY: ");
  128. PrintHex<uint8_t >(y, 0x80);
  129. Serial.println("");
  130. #endif
  131. uint8_t newX = centerMouseX;
  132. uint8_t newY = centerMouseY;
  133. #ifdef DEBUG_OUTPUT
  134. uint8_t updated = 0;
  135. #endif
  136. if ((x > (centerMouseX + deadZoneMouseX))
  137. || (x < (centerMouseX - deadZoneMouseX))) {
  138. newX = x;
  139. #ifdef DEBUG_OUTPUT
  140. updated = 1;
  141. #endif
  142. }
  143. if ((y > (centerMouseY + deadZoneMouseY))
  144. || (y < (centerMouseY - deadZoneMouseY))) {
  145. newY = y;
  146. #ifdef DEBUG_OUTPUT
  147. updated = 1;
  148. #endif
  149. }
  150. #ifdef DEBUG_OUTPUT
  151. if (updated) {
  152. Serial.print("Mouse X: ");
  153. PrintHex<uint8_t >(newX, 0x80);
  154. Serial.print("\tY: ");
  155. PrintHex<uint8_t >(newY, 0x80);
  156. Serial.println("");
  157. }
  158. #endif
  159. if (client) {
  160. client->OnMouseMoved(x, y);
  161. }
  162. }