Control drones with a proper joystick
Вы не можете выбрать более 25 тем Темы должны начинаться с буквы или цифры, могут содержать дефисы(-) и должны содержать не более 35 символов.

parser.cpp 3.3KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117
  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 "events.h"
  13. #include "parser.h"
  14. //#define DEBUG_OUTPUT Serial
  15. JoystickReportParser::JoystickReportParser(JoystickEvents* evt)
  16. : joyEvents(evt), oldHat(0), oldButtons(0), oldMouse(0), buffer(0) {
  17. for (uint8_t i = 0; i < RPT_GEMEPAD_LEN; i++) {
  18. oldPad[i] = 0;
  19. }
  20. }
  21. void JoystickReportParser::Parse(USBHID* hid, bool is_rpt_id, uint8_t len, uint8_t* bufPart) {
  22. // Ugly hack for too small packet size in USB Host library...
  23. if (len == 8) {
  24. // First part of buffer, store and do nothing
  25. for (uint8_t i = 0; i < 8; i++) {
  26. buf[i] = bufPart[i];
  27. }
  28. return;
  29. } else {
  30. // Append second part, then evaluate
  31. for (uint8_t i = 0; i < len; i++) {
  32. buf[i + 8] = bufPart[i];
  33. }
  34. }
  35. #ifdef DEBUG_OUTPUT
  36. // Dump whole USB HID packet for debugging purposes
  37. DEBUG_OUTPUT.println("");
  38. DEBUG_OUTPUT.print("Packet: ");
  39. for (uint8_t i = 0; i < (8 + len); i++) {
  40. DEBUG_OUTPUT.print(buf[i], HEX);
  41. DEBUG_OUTPUT.print(" ");
  42. }
  43. DEBUG_OUTPUT.println("");
  44. DEBUG_OUTPUT.println("");
  45. #endif
  46. // Checking if there are changes in report since the method was last called
  47. bool match = true;
  48. for (uint8_t i = 0; i < RPT_GEMEPAD_LEN; i++) {
  49. if (buf[i] != oldPad[i]) {
  50. match = false;
  51. break;
  52. }
  53. }
  54. // Calling Game Pad event handler
  55. if (!match && joyEvents) {
  56. buffer.X = buf[0] | ((buf[1] & 0x07) << 8);
  57. buffer.Y = ((buf[1] & 0xF8) >> 3) | ((buf[2] & 0x3F) << 5);
  58. buffer.Rz = ((buf[2] & 0xC0) >> 6) | (buf[3] << 2);
  59. buffer.Z = buf[4];
  60. buffer.Rx = buf[5];
  61. buffer.Ry = buf[6];
  62. buffer.Slider = buf[7];
  63. joyEvents->OnGamePadChanged(buffer);
  64. for (uint8_t i = 0; i < RPT_GEMEPAD_LEN; i++) {
  65. oldPad[i] = buf[i];
  66. }
  67. }
  68. // Calling Hat Switch event handler
  69. uint8_t hat = (buf[12] & 0xF0) >> 4;
  70. if (hat != oldHat && joyEvents) {
  71. joyEvents->OnHatSwitch(hat);
  72. oldHat = hat;
  73. }
  74. uint64_t buttons = buf[12] & 0x03;
  75. buttons <<= 8;
  76. buttons |= buf[11];
  77. buttons <<= 8;
  78. buttons |= buf[10];
  79. buttons <<= 8;
  80. buttons |= buf[9];
  81. buttons <<= 8;
  82. buttons |= buf[8];
  83. // Calling Button Event Handler for every button changed
  84. uint64_t changes = (buttons ^ oldButtons);
  85. if (changes) {
  86. for (uint8_t i = 0; i < 34; i++) {
  87. uint64_t mask = (1ull << i);
  88. if ((mask & changes) && joyEvents) {
  89. if (buttons & mask) {
  90. joyEvents->OnButtonDown(i);
  91. } else {
  92. joyEvents->OnButtonUp(i);
  93. }
  94. }
  95. }
  96. oldButtons = buttons;
  97. }
  98. // Calling Mouse Event Handler if state has changed
  99. if (oldMouse != buf[13] && joyEvents) {
  100. oldMouse = buf[13];
  101. joyEvents->OnMouseMoved((buf[13] & 0xF0) >> 4, buf[13] & 0x0F);
  102. }
  103. }