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.

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 <Arduino.h>
  13. #include "data.h"
  14. #include "cppm.h"
  15. #include "events.h"
  16. JoystickEventsCPPM::JoystickEventsCPPM(JoystickEvents* client) : JoystickEvents(client) {
  17. for (uint8_t i = 0; i < CHANNELS_MAX; i++) {
  18. values[i] = CHANNEL_DEFAULT_VALUE;
  19. invert[i] = 0;
  20. minimum[i] = CHANNEL_MINIMUM_VALUE;
  21. maximum[i] = CHANNEL_MAXIMUM_VALUE;
  22. trim[i] = 0;
  23. }
  24. /*
  25. * Aux switches are commonly used for arming.
  26. * Ensure we're not sending high values when
  27. * no joystick has been connected...
  28. */
  29. values[CHANNEL_AUX1] = CHANNEL_MINIMUM_VALUE;
  30. values[CHANNEL_AUX2] = CHANNEL_MINIMUM_VALUE;
  31. CPPM::instance().copy(values);
  32. }
  33. void JoystickEventsCPPM::OnGamePadChanged(const GamePadEventData& evt) {
  34. // up to CHANNEL_AUX2 we are using axes
  35. for (uint8_t i = 0; i < (CHANNEL_AUX2 + 1); i++) {
  36. uint16_t value = ((int32_t)getJoystickAxis(evt, i)) + trim[i];
  37. values[i] = map(value, 0, getJoystickMax(i),
  38. invert[i] ? maximum[i] : minimum[i], invert[i] ? minimum[i] : maximum[i]);
  39. }
  40. CPPM::instance().copy(values);
  41. if (client) {
  42. client->OnGamePadChanged(evt);
  43. }
  44. }
  45. void JoystickEventsCPPM::OnButtonUp(uint8_t but_id) {
  46. // if you want to do something when a button is released
  47. // do it here...
  48. if (client) {
  49. client->OnButtonUp(but_id);
  50. }
  51. }
  52. void JoystickEventsCPPM::OnButtonDown(uint8_t but_id) {
  53. // called when a button is pressed
  54. if (but_id == 42) { // TODO proper button id
  55. // set CHANNEL_AUX3 according to button
  56. values[CHANNEL_AUX3] = 1000;
  57. } else if (but_id == 23) { // TODO proper button id
  58. values[CHANNEL_AUX3] = 2000;
  59. }
  60. CPPM::instance().copy(values);
  61. if (client) {
  62. client->OnButtonDown(but_id);
  63. }
  64. }
  65. uint16_t JoystickEventsCPPM::getJoystickAxis(const GamePadEventData& evt, uint8_t ch) {
  66. if (ch == CHANNEL_ROLL) {
  67. return evt.X;
  68. } else if (ch == CHANNEL_PITCH) {
  69. return evt.Y;
  70. } else if (ch == CHANNEL_THROTTLE) {
  71. return evt.Z;
  72. } else if (ch == CHANNEL_YAW) {
  73. return evt.Rz;
  74. } else if (ch == CHANNEL_AUX1) {
  75. return evt.Ry;
  76. } else if (ch == CHANNEL_AUX2) {
  77. return evt.Slider;
  78. } else if (ch == (CHANNEL_AUX2 + 1)) {
  79. return evt.Rx;
  80. } else {
  81. return 0;
  82. }
  83. }
  84. uint16_t JoystickEventsCPPM::getJoystickMax(uint8_t ch) {
  85. if (ch == CHANNEL_ROLL) {
  86. return 0x7FF;
  87. } else if (ch == CHANNEL_PITCH) {
  88. return 0x7FF;
  89. } else if (ch == CHANNEL_THROTTLE) {
  90. return 0xFF;
  91. } else if (ch == CHANNEL_YAW) {
  92. return 0x3FF;
  93. } else if (ch == CHANNEL_AUX1) {
  94. return 0xFF;
  95. } else if (ch == CHANNEL_AUX2) {
  96. return 0xFF;
  97. } else if (ch == (CHANNEL_AUX2 + 1)) {
  98. return 0xFF;
  99. } else {
  100. return 0xFF;
  101. }
  102. }