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_cppm.cpp 2.6KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091
  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. for (uint8_t i = 0; i < (CHANNEL_AUX2 + 1); i++) {
  35. uint16_t value = ((int32_t)getJoystickAxis(evt, i)) + trim[i];
  36. values[i] = map(value, 0, getJoystickMax(i),
  37. invert[i] ? maximum[i] : minimum[i], invert[i] ? minimum[i] : maximum[i]);
  38. }
  39. CPPM::instance().copy(values);
  40. if (client) {
  41. client->OnGamePadChanged(evt);
  42. }
  43. }
  44. uint16_t JoystickEventsCPPM::getJoystickAxis(const GamePadEventData& evt, uint8_t ch) {
  45. if (ch == CHANNEL_ROLL) {
  46. return evt.X;
  47. } else if (ch == CHANNEL_PITCH) {
  48. return evt.Y;
  49. } else if (ch == CHANNEL_THROTTLE) {
  50. return evt.Z;
  51. } else if (ch == CHANNEL_YAW) {
  52. return evt.Rz;
  53. } else if (ch == CHANNEL_AUX1) {
  54. return evt.Ry;
  55. } else if (ch == CHANNEL_AUX2) {
  56. return evt.Slider;
  57. } else if (ch == (CHANNEL_AUX2 + 1)) {
  58. return evt.Rx;
  59. } else {
  60. return 0;
  61. }
  62. }
  63. uint16_t JoystickEventsCPPM::getJoystickMax(uint8_t ch) {
  64. if (ch == CHANNEL_ROLL) {
  65. return 0x7FF;
  66. } else if (ch == CHANNEL_PITCH) {
  67. return 0x7FF;
  68. } else if (ch == CHANNEL_THROTTLE) {
  69. return 0xFF;
  70. } else if (ch == CHANNEL_YAW) {
  71. return 0x3FF;
  72. } else if (ch == CHANNEL_AUX1) {
  73. return 0xFF;
  74. } else if (ch == CHANNEL_AUX2) {
  75. return 0xFF;
  76. } else if (ch == (CHANNEL_AUX2 + 1)) {
  77. return 0xFF;
  78. } else {
  79. return 0xFF;
  80. }
  81. }