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.

hid_parser.cpp 3.3KB

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