Control drones with a proper joystick
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

events_buttons.cpp 17KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476
  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 "x52.h"
  15. #include "cppm.h"
  16. #include "events.h"
  17. #include "config.h"
  18. //#define DEBUG_OUTPUT Serial
  19. //#define DEBUG_BUTTON_MFD
  20. #define MENU_BUTTON_ENTER_1 29
  21. #define MENU_BUTTON_ENTER_2 26
  22. #define MENU_BUTTON_DOWN 27
  23. #define MENU_BUTTON_UP 28
  24. #define MODE_BUTTON_GREEN 23
  25. #define MODE_BUTTON_YELLOW 24
  26. #define MODE_BUTTON_RED 25
  27. #define MIN_FRAME_LENGTH 10000
  28. #define MAX_FRAME_LENGTH 30000
  29. #define MIN_PULSE_LENGTH 100
  30. #define MAX_PULSE_LENGTH 1000
  31. #define MIN_LOW_ENDPOINT 500
  32. #define MAX_LOW_ENDPOINT 1500
  33. #define MIN_HIGH_ENDPOINT 1500
  34. #define MAX_HIGH_ENDPOINT 2500
  35. #define MIN_TRIM -100
  36. #define MAX_TRIM 100
  37. void statusCallback(uint8_t a1, uint8_t a2, uint8_t q1, uint8_t q2);
  38. JoystickEventsButtons::JoystickEventsButtons(JoystickEvents* client)
  39. : JoystickEvents(client), state(NONE), index(0),
  40. value(0), signedValue(0), currentMode(0) { }
  41. void JoystickEventsButtons::printMenu() {
  42. static const char* mainMenu[] = {
  43. "Status", "Trim Axis", "Trim Endpoint", "Invert Axis", "CPPM Config",
  44. "Save EEPROM", "Load EEPROM"
  45. };
  46. static const uint8_t mainMenuCount = sizeof(mainMenu) / sizeof(mainMenu[0]);
  47. static const char* cppmMenu[] = {
  48. "Channels", "Frame Length", "Pulse Length", "Invert", "Output Pin", "Main Menu"
  49. };
  50. static const uint8_t cppmMenuCount = sizeof(cppmMenu) / sizeof(cppmMenu[0]);
  51. static const char* axisMenu[] = {
  52. "Roll", "Pitch", "Yaw", "Throttle", "Aux1", "Aux2", "Main Menu"
  53. };
  54. static const uint8_t axisMenuCount = sizeof(axisMenu) / sizeof(axisMenu[0]);
  55. static const char* endpointMenu[] = {
  56. "Roll Min", "Roll Max", "Pitch Min", "Pitch Max", "Yaw Min", "Yaw Max",
  57. "Throttle Min", "Throttle Max", "Aux1 Min", "Aux1 Max", "Aux2 Min",
  58. "Aux2 Max", "Main Menu"
  59. };
  60. static const uint8_t endpointMenuCount = sizeof(endpointMenu) / sizeof(endpointMenu[0]);
  61. if (state == NONE) {
  62. statusCallback(0, 0, 0, 0);
  63. } else if (state == MAINMENU) {
  64. menuHelper(mainMenuCount, mainMenu, "Main Menu");
  65. } else if (state == CPPMMENU) {
  66. menuHelper(cppmMenuCount, cppmMenu, "CPPM Config Menu");
  67. } else if (state == TRIMAXISMENU) {
  68. menuHelper(axisMenuCount, axisMenu, "Trim Axis Menu");
  69. } else if (state == TRIMENDPOINTMENU) {
  70. menuHelper(endpointMenuCount, endpointMenu, "Trim Endpoints");
  71. } else if (state == INVERTAXISMENU) {
  72. menuHelper(axisMenuCount, axisMenu, "Invert Axis Menu");
  73. } else if (state == EDIT_CHANNELS) {
  74. printValue(4, CHANNELS_MAX, cppmMenu[0]);
  75. } else if (state == EDIT_FRAME_LENGTH) {
  76. printValue(MIN_FRAME_LENGTH, MAX_FRAME_LENGTH, cppmMenu[1]);
  77. } else if (state == EDIT_PULSE_LENGTH) {
  78. printValue(MIN_PULSE_LENGTH, MAX_PULSE_LENGTH, cppmMenu[2]);
  79. } else if (state == EDIT_INVERT) {
  80. printValue(0, 1, cppmMenu[3]);
  81. } else if (state == EDIT_CPPM_PIN) {
  82. printValue(0, 13, cppmMenu[4]);
  83. } else if ((state >= EDIT_INVERT_ROLL) && (state <= EDIT_INVERT_AUX2)) {
  84. uint8_t index = state - EDIT_INVERT_ROLL;
  85. printValue(0, 1, (String("Invert ") + axisMenu[index]).c_str());
  86. } else if ((state >= EDIT_MIN_ROLL) && (state <= EDIT_MAX_AUX2)) {
  87. uint8_t index = state - EDIT_MIN_ROLL;
  88. if (index & 0x01) {
  89. printValue(MIN_HIGH_ENDPOINT, MAX_HIGH_ENDPOINT, endpointMenu[index]);
  90. } else {
  91. printValue(MIN_LOW_ENDPOINT, MAX_LOW_ENDPOINT, endpointMenu[index]);
  92. }
  93. } else if ((state >= EDIT_TRIM_ROLL) && (state <= EDIT_TRIM_AUX2)) {
  94. uint8_t index = state - EDIT_TRIM_ROLL;
  95. printSignedValue(MIN_TRIM, MAX_TRIM, (String("Trim ") + axisMenu[index]).c_str());
  96. }
  97. }
  98. void JoystickEventsButtons::OnButtonDown(uint8_t but_id) {
  99. #ifdef DEBUG_BUTTON_MFD
  100. String text = "Button " + String(but_id) + " down";
  101. x52.setMFDText(1, text.c_str());
  102. #endif
  103. if (but_id == MODE_BUTTON_GREEN) {
  104. x52.setLEDBrightness(2);
  105. x52.setMFDBrightness(2);
  106. currentMode = 1;
  107. } else if (but_id == MODE_BUTTON_YELLOW) {
  108. x52.setLEDBrightness(1);
  109. x52.setMFDBrightness(1);
  110. currentMode = 2;
  111. } else if (but_id == MODE_BUTTON_RED) {
  112. x52.setLEDBrightness(0);
  113. x52.setMFDBrightness(0);
  114. currentMode = 3;
  115. } else if ((but_id == MENU_BUTTON_ENTER_1) || (but_id == MENU_BUTTON_ENTER_2)) {
  116. if (state == NONE) {
  117. state = MAINMENU;
  118. index = 0;
  119. } else if (state == MAINMENU) {
  120. if (index == 0) {
  121. state = NONE;
  122. } else if (index == 1) {
  123. state = TRIMAXISMENU;
  124. index = 0;
  125. } else if (index == 2) {
  126. state = TRIMENDPOINTMENU;
  127. index = 0;
  128. } else if (index == 3) {
  129. state = INVERTAXISMENU;
  130. index = 0;
  131. } else if (index == 4) {
  132. state = CPPMMENU;
  133. index = 0;
  134. } else if (index == 5) {
  135. eepromWrite();
  136. index = 0;
  137. } else if (index == 6) {
  138. eepromRead();
  139. index = 0;
  140. }
  141. } else if (state == TRIMAXISMENU) {
  142. if (index == 0) {
  143. state = EDIT_TRIM_ROLL;
  144. signedValue = joyCPPM.getTrim(CHANNEL_ROLL);
  145. } else if (index == 1) {
  146. state = EDIT_TRIM_PITCH;
  147. signedValue = joyCPPM.getTrim(CHANNEL_PITCH);
  148. } else if (index == 2) {
  149. state = EDIT_TRIM_YAW;
  150. signedValue = joyCPPM.getTrim(CHANNEL_YAW);
  151. } else if (index == 3) {
  152. state = EDIT_TRIM_THROTTLE;
  153. signedValue = joyCPPM.getTrim(CHANNEL_THROTTLE);
  154. } else if (index == 4) {
  155. state = EDIT_TRIM_AUX1;
  156. signedValue = joyCPPM.getTrim(CHANNEL_AUX1);
  157. } else if (index == 5) {
  158. state = EDIT_TRIM_AUX2;
  159. signedValue = joyCPPM.getTrim(CHANNEL_AUX2);
  160. } else if (index == 6) {
  161. state = MAINMENU;
  162. index = 0;
  163. }
  164. } else if (state == TRIMENDPOINTMENU) {
  165. if (index == 0) {
  166. state = EDIT_MIN_ROLL;
  167. value = joyCPPM.getMinimum(CHANNEL_ROLL);
  168. } else if (index == 1) {
  169. state = EDIT_MAX_ROLL;
  170. value = joyCPPM.getMaximum(CHANNEL_ROLL);
  171. } else if (index == 2) {
  172. state = EDIT_MIN_PITCH;
  173. value = joyCPPM.getMinimum(CHANNEL_PITCH);
  174. } else if (index == 3) {
  175. state = EDIT_MAX_PITCH;
  176. value = joyCPPM.getMaximum(CHANNEL_PITCH);
  177. } else if (index == 4) {
  178. state = EDIT_MIN_YAW;
  179. value = joyCPPM.getMinimum(CHANNEL_YAW);
  180. } else if (index == 5) {
  181. state = EDIT_MAX_YAW;
  182. value = joyCPPM.getMaximum(CHANNEL_YAW);
  183. } else if (index == 6) {
  184. state = EDIT_MIN_THROTTLE;
  185. value = joyCPPM.getMinimum(CHANNEL_THROTTLE);
  186. } else if (index == 7) {
  187. state = EDIT_MAX_THROTTLE;
  188. value = joyCPPM.getMaximum(CHANNEL_THROTTLE);
  189. } else if (index == 8) {
  190. state = EDIT_MIN_AUX1;
  191. value = joyCPPM.getMinimum(CHANNEL_AUX1);
  192. } else if (index == 9) {
  193. state = EDIT_MAX_AUX1;
  194. value = joyCPPM.getMaximum(CHANNEL_AUX1);
  195. } else if (index == 10) {
  196. state = EDIT_MIN_AUX2;
  197. value = joyCPPM.getMinimum(CHANNEL_AUX2);
  198. } else if (index == 11) {
  199. state = EDIT_MAX_AUX2;
  200. value = joyCPPM.getMaximum(CHANNEL_AUX2);
  201. } else if (index == 12) {
  202. state = EDIT_MIN_ROLL;
  203. state = MAINMENU;
  204. index = 0;
  205. }
  206. } else if (state == INVERTAXISMENU) {
  207. if (index == 0) {
  208. state = EDIT_INVERT_ROLL;
  209. value = joyCPPM.getInvert(CHANNEL_ROLL);
  210. } else if (index == 1) {
  211. state = EDIT_INVERT_PITCH;
  212. value = joyCPPM.getInvert(CHANNEL_PITCH);
  213. } else if (index == 2) {
  214. state = EDIT_INVERT_YAW;
  215. value = joyCPPM.getInvert(CHANNEL_YAW);
  216. } else if (index == 3) {
  217. state = EDIT_INVERT_THROTTLE;
  218. value = joyCPPM.getInvert(CHANNEL_THROTTLE);
  219. } else if (index == 4) {
  220. state = EDIT_INVERT_AUX1;
  221. value = joyCPPM.getInvert(CHANNEL_AUX1);
  222. } else if (index == 5) {
  223. state = EDIT_INVERT_AUX2;
  224. value = joyCPPM.getInvert(CHANNEL_AUX2);
  225. } else if (index == 6) {
  226. state = MAINMENU;
  227. index = 0;
  228. }
  229. } else if (state == CPPMMENU) {
  230. if (index == 0) {
  231. state = EDIT_CHANNELS;
  232. value = CPPM::instance().getChannels();
  233. } else if (index == 1) {
  234. state = EDIT_FRAME_LENGTH;
  235. value = CPPM::instance().getFrameLength();
  236. } else if (index == 2) {
  237. state = EDIT_PULSE_LENGTH;
  238. value = CPPM::instance().getPulseLength();
  239. } else if (index == 3) {
  240. state = EDIT_INVERT;
  241. value = CPPM::instance().getInvert();
  242. } else if (index == 4) {
  243. state = EDIT_CPPM_PIN;
  244. value = CPPM::instance().getOutput();
  245. } else if (index == 5) {
  246. state = MAINMENU;
  247. index = 0;
  248. }
  249. } else if (state == EDIT_CHANNELS) {
  250. CPPM::instance().setChannels(value);
  251. state = CPPMMENU;
  252. } else if (state == EDIT_FRAME_LENGTH) {
  253. CPPM::instance().setFrameLength(value);
  254. state = CPPMMENU;
  255. } else if (state == EDIT_PULSE_LENGTH) {
  256. CPPM::instance().setPulseLength(value);
  257. state = CPPMMENU;
  258. } else if (state == EDIT_INVERT) {
  259. CPPM::instance().setInvert(value);
  260. state = CPPMMENU;
  261. } else if (state == EDIT_CPPM_PIN) {
  262. CPPM::instance().setOutput(value);
  263. state = CPPMMENU;
  264. } else if (state == EDIT_INVERT_ROLL) {
  265. joyCPPM.setInvert(CHANNEL_ROLL, value);
  266. state = INVERTAXISMENU;
  267. } else if (state == EDIT_INVERT_PITCH) {
  268. joyCPPM.setInvert(CHANNEL_PITCH, value);
  269. state = INVERTAXISMENU;
  270. } else if (state == EDIT_INVERT_YAW) {
  271. joyCPPM.setInvert(CHANNEL_YAW, value);
  272. state = INVERTAXISMENU;
  273. } else if (state == EDIT_INVERT_THROTTLE) {
  274. joyCPPM.setInvert(CHANNEL_THROTTLE, value);
  275. state = INVERTAXISMENU;
  276. } else if (state == EDIT_INVERT_AUX1) {
  277. joyCPPM.setInvert(CHANNEL_AUX1, value);
  278. state = INVERTAXISMENU;
  279. } else if (state == EDIT_INVERT_AUX2) {
  280. joyCPPM.setInvert(CHANNEL_AUX2, value);
  281. state = INVERTAXISMENU;
  282. } else if (state == EDIT_MIN_ROLL) {
  283. joyCPPM.setMinimum(CHANNEL_ROLL, value);
  284. state = TRIMENDPOINTMENU;
  285. } else if (state == EDIT_MAX_ROLL) {
  286. joyCPPM.setMaximum(CHANNEL_ROLL, value);
  287. state = TRIMENDPOINTMENU;
  288. } else if (state == EDIT_MIN_PITCH) {
  289. joyCPPM.setMinimum(CHANNEL_PITCH, value);
  290. state = TRIMENDPOINTMENU;
  291. } else if (state == EDIT_MAX_PITCH) {
  292. joyCPPM.setMaximum(CHANNEL_PITCH, value);
  293. state = TRIMENDPOINTMENU;
  294. } else if (state == EDIT_MIN_YAW) {
  295. joyCPPM.setMinimum(CHANNEL_YAW, value);
  296. state = TRIMENDPOINTMENU;
  297. } else if (state == EDIT_MAX_YAW) {
  298. joyCPPM.setMaximum(CHANNEL_YAW, value);
  299. state = TRIMENDPOINTMENU;
  300. } else if (state == EDIT_MIN_THROTTLE) {
  301. joyCPPM.setMinimum(CHANNEL_THROTTLE, value);
  302. state = TRIMENDPOINTMENU;
  303. } else if (state == EDIT_MAX_THROTTLE) {
  304. joyCPPM.setMaximum(CHANNEL_THROTTLE, value);
  305. state = TRIMENDPOINTMENU;
  306. } else if (state == EDIT_MIN_AUX1) {
  307. joyCPPM.setMinimum(CHANNEL_AUX1, value);
  308. state = TRIMENDPOINTMENU;
  309. } else if (state == EDIT_MAX_AUX1) {
  310. joyCPPM.setMaximum(CHANNEL_AUX1, value);
  311. state = TRIMENDPOINTMENU;
  312. } else if (state == EDIT_MIN_AUX2) {
  313. joyCPPM.setMinimum(CHANNEL_AUX2, value);
  314. state = TRIMENDPOINTMENU;
  315. } else if (state == EDIT_MAX_AUX2) {
  316. joyCPPM.setMaximum(CHANNEL_AUX2, value);
  317. state = TRIMENDPOINTMENU;
  318. } else if (state == EDIT_TRIM_ROLL) {
  319. joyCPPM.setTrim(CHANNEL_ROLL, signedValue);
  320. state = TRIMAXISMENU;
  321. } else if (state == EDIT_TRIM_PITCH) {
  322. joyCPPM.setTrim(CHANNEL_PITCH, signedValue);
  323. state = TRIMAXISMENU;
  324. } else if (state == EDIT_TRIM_YAW) {
  325. joyCPPM.setTrim(CHANNEL_YAW, signedValue);
  326. state = TRIMAXISMENU;
  327. } else if (state == EDIT_TRIM_THROTTLE) {
  328. joyCPPM.setTrim(CHANNEL_THROTTLE, signedValue);
  329. state = TRIMAXISMENU;
  330. } else if (state == EDIT_TRIM_AUX1) {
  331. joyCPPM.setTrim(CHANNEL_AUX1, signedValue);
  332. state = TRIMAXISMENU;
  333. } else if (state == EDIT_TRIM_AUX2) {
  334. joyCPPM.setTrim(CHANNEL_AUX2, signedValue);
  335. state = TRIMAXISMENU;
  336. }
  337. printMenu();
  338. } else if (but_id == MENU_BUTTON_DOWN) {
  339. if ((state > STATES_EDIT) && (state < STATES_EDIT_SIGNED)) {
  340. if (value > 0) {
  341. value--;
  342. }
  343. } else if (state > STATES_EDIT_SIGNED) {
  344. if (signedValue > -32767) {
  345. signedValue--;
  346. }
  347. } else if (state != NONE) {
  348. index++;
  349. }
  350. printMenu();
  351. } else if (but_id == MENU_BUTTON_UP) {
  352. if ((state > STATES_EDIT) && (state < STATES_EDIT_SIGNED)) {
  353. if (value < 0xFFFF) {
  354. value++;
  355. }
  356. } else if (state > STATES_EDIT_SIGNED) {
  357. if (signedValue < 32767) {
  358. signedValue++;
  359. }
  360. } else if (state != NONE) {
  361. if (index > 0) {
  362. index--;
  363. }
  364. }
  365. printMenu();
  366. }
  367. if (client) {
  368. client->OnButtonDown(but_id);
  369. }
  370. }
  371. void JoystickEventsButtons::menuHelper(uint8_t count, const char** menu, const char* title) {
  372. if (index >= count) {
  373. index = count - 1;
  374. }
  375. uint8_t start = 0, line = 0;
  376. if (index > 1) {
  377. start = index - 1;
  378. }
  379. uint8_t end = start + 2;
  380. if (index == 0) {
  381. x52.setMFDText(0, title);
  382. line = 1;
  383. end = start + 1;
  384. }
  385. if (end >= count) {
  386. end = count - 1;
  387. }
  388. for (uint8_t i = start; i <= end; i++) {
  389. String tmp = (index == i) ? "-> " : " ";
  390. x52.setMFDText(line++, (tmp + menu[i]).c_str());
  391. }
  392. if (line == 2) {
  393. x52.setMFDText(2);
  394. }
  395. #ifdef DEBUG_OUTPUT
  396. DEBUG_OUTPUT.print("menuHelper() state=");
  397. DEBUG_OUTPUT.print(state);
  398. DEBUG_OUTPUT.print(" index=");
  399. DEBUG_OUTPUT.print(index);
  400. DEBUG_OUTPUT.print(" start=");
  401. DEBUG_OUTPUT.print(start);
  402. DEBUG_OUTPUT.print(" end=");
  403. DEBUG_OUTPUT.println(end);
  404. #endif
  405. }
  406. void JoystickEventsButtons::printValue(uint16_t min, uint16_t max, const char* title) {
  407. #ifdef DEBUG_OUTPUT
  408. DEBUG_OUTPUT.print("printValue() state=");
  409. DEBUG_OUTPUT.print(state);
  410. DEBUG_OUTPUT.print(" index=");
  411. DEBUG_OUTPUT.print(index);
  412. DEBUG_OUTPUT.print(" min=");
  413. DEBUG_OUTPUT.print(min);
  414. DEBUG_OUTPUT.print(" max=");
  415. DEBUG_OUTPUT.println(max);
  416. #endif
  417. if (value < min) {
  418. value = min;
  419. }
  420. if (value > max) {
  421. value = max;
  422. }
  423. x52.setMFDText(0, (String(title) + ":").c_str());
  424. x52.setMFDText(1, String(value).c_str());
  425. x52.setMFDText(2, "Press OK to save");
  426. }
  427. void JoystickEventsButtons::printSignedValue(int16_t min, int16_t max, const char* title) {
  428. #ifdef DEBUG_OUTPUT
  429. DEBUG_OUTPUT.print("printSignedValue() state=");
  430. DEBUG_OUTPUT.print(state);
  431. DEBUG_OUTPUT.print(" index=");
  432. DEBUG_OUTPUT.print(index);
  433. DEBUG_OUTPUT.print(" min=");
  434. DEBUG_OUTPUT.print(min);
  435. DEBUG_OUTPUT.print(" max=");
  436. DEBUG_OUTPUT.println(max);
  437. #endif
  438. if (signedValue < min) {
  439. signedValue = min;
  440. }
  441. if (signedValue > max) {
  442. signedValue = max;
  443. }
  444. x52.setMFDText(0, (String(title) + ":").c_str());
  445. x52.setMFDText(1, String(signedValue).c_str());
  446. x52.setMFDText(2, "Press OK to save");
  447. }