Debian Codegraph @

libs/hwdrivers/src/CJoystick.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/config.h>
13 #include <mrpt/core/exceptions.h>
14
15 #ifdef _WIN32
16 #define WIN32_LEAN_AND_MEAN
17 #include <windows.h>
18 //
19 #include <mmsystem.h>
20
21 #if !defined(__GNUC__)
22 #pragma comment(lib, "WINMM.LIB")
23 #endif
24 #endif
25
26 #if defined(MRPT_OS_LINUX) || defined(__APPLE__)
27 // Linux
28 #include <fcntl.h>
29 #include <sys/ioctl.h>
30 #include <sys/time.h>
31 #include <sys/types.h>
32 #include <unistd.h>
33
34 #include <cerrno>
35 #include <cstdio>
36 #include <cstdlib>
37 #include <cstring>
38
39 #if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H)
40 #include <linux/input.h>
41 #include <linux/joystick.h>
42 #endif
43 #endif
44
45 #include <mrpt/hwdrivers/CJoystick.h>
46
47 using namespace mrpt::hwdrivers;
48
49 CJoystick::CJoystick() = default;
50
51 /*---------------------------------------------------------------
52 Destructor
53 ---------------------------------------------------------------*/
54 CJoystick::~CJoystick()
55 {
56 #if defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H)
57 // Close joystick, if open:
58 if (m_joy_fd > 0)
59 {
60 close(m_joy_fd);
61 m_joy_fd = -1;
62 }
63 #endif
64 }
65
66 /*---------------------------------------------------------------
67 getJoysticksCount
68 Returns the number of Joysticks in the computer.
69 ---------------------------------------------------------------*/
70 int CJoystick::getJoysticksCount()
71 {
72 MRPT_START
73 #ifdef _WIN32
74 return joyGetNumDevs();
75 #elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H)
76 // Try to open several joy devs:
77 int joy_fd = -1;
78 int nJoys = 0;
79
80 do
81 {
82 if (-1 != (joy_fd = open(format("/dev/input/js%i", nJoys).c_str(), O_RDONLY)))
83 {
84 nJoys++;
85 close(joy_fd);
86 }
87 } while (joy_fd != -1);
88
89 return nJoys;
90 #else
91 // Apple:
92 return 0;
93 #endif
94 MRPT_END
95 }
96
97 /*---------------------------------------------------------------
98 Gets joystick information.
99
100 \return Returns true if successful, false on error, for example, if joystick
101 is not present.
102 ---------------------------------------------------------------*/
103 bool CJoystick::getJoystickPosition(int nJoy, State& output)
104 {
105 MRPT_START
106 #ifdef _WIN32
107 JOYINFO jinfo;
108
109 int ID = JOYSTICKID1 + nJoy;
110
111 // Get joy pos:
112 if (JOYERR_NOERROR != joyGetPos(ID, &jinfo)) return false; // Error.
113
114 int calib_min[3], calib_max[3];
115
116 for (int i = 0; i < 3; i++)
117 {
118 calib_min[i] = m_minPerAxis.size() > i ? m_minPerAxis[i] : 0;
119 calib_max[i] = m_maxPerAxis.size() > i ? m_maxPerAxis[i] : 0xffff;
120 }
121
122 // Output data:
123 float x = (jinfo.wXpos - calib_min[0]) / (float)(calib_max[0] - calib_min[0]);
124 float y = (jinfo.wYpos - calib_min[1]) / (float)(calib_max[1] - calib_min[1]);
125 float z = (jinfo.wZpos - calib_min[2]) / (float)(calib_max[2] - calib_min[2]);
126
127 x = 2 * x - 1;
128 y = 2 * y - 1;
129 z = 2 * z - 1;
130
131 output.buttons.resize(4);
132
133 output.buttons[0] = 0 != (jinfo.wButtons & JOY_BUTTON1);
134 output.buttons[1] = 0 != (jinfo.wButtons & JOY_BUTTON2);
135 output.buttons[2] = 0 != (jinfo.wButtons & JOY_BUTTON3);
136 output.buttons[3] = 0 != (jinfo.wButtons & JOY_BUTTON4);
137
138 output.axes_raw.resize(3);
139 output.axes_raw[0] = jinfo.wXpos;
140 output.axes_raw[1] = jinfo.wYpos;
141 output.axes_raw[2] = jinfo.wZpos;
142
143 output.axes.resize(3);
144 output.axes[0] = x;
145 output.axes[1] = y;
146 output.axes[2] = z;
147
148 return true;
149 #elif defined(MRPT_OS_LINUX) && defined(HAVE_LINUX_INPUT_H)
150 // Already open?
151 if (m_joy_index == nJoy && m_joy_fd != -1)
152 {
153 // Ok
154 }
155 else
156 {
157 // Close previous opened joy?
158 if (m_joy_fd != -1) close(m_joy_fd);
159
160 // Go, try open joystick:
161 if ((m_joy_fd = open(format("/dev/input/js%i", nJoy).c_str(), O_RDONLY)) < 0) return false;
162
163 // Perfect!
164 m_joy_index = nJoy;
165
166 // Read in non-blocking way: **** Refer to sources of "jstest"!!!! ***
167 fcntl(m_joy_fd, F_SETFL, O_NONBLOCK);
168 }
169
170 struct js_event js;
171
172 while (read(m_joy_fd, &js, sizeof(struct js_event)) == sizeof(struct js_event))
173 {
174 // js.number: Button number
175 const size_t jsNum = static_cast<size_t>(js.number);
176
177 // Button?
178 if (js.type & JS_EVENT_BUTTON)
179 {
180 if (m_joystate_btns.size() < jsNum + 1) m_joystate_btns.resize(jsNum + 1);
181
182 m_joystate_btns[jsNum] = (js.value != 0);
183 }
184
185 // Axes?
186 if (js.type & JS_EVENT_AXIS)
187 {
188 if (m_joystate_axes.size() < jsNum + 1) m_joystate_axes.resize(jsNum + 1);
189
190 m_joystate_axes[jsNum] = static_cast<int>(js.value);
191 }
192 }
193
194 if (errno != EAGAIN)
195 {
196 // Joystick disconnected?
197 m_joy_fd = -1;
198 m_joy_index = -1;
199 return false;
200 }
201
202 // Fill out data:
203 output.buttons = m_joystate_btns;
204
205 const size_t nAxis = m_joystate_axes.size();
206 output.axes_raw.resize(nAxis);
207 output.axes.resize(nAxis);
208
209 for (size_t i = 0; i < nAxis; i++)
210 {
211 output.axes_raw[i] = m_joystate_axes[i];
212
213 const int calib_min = m_minPerAxis.size() > i ? m_minPerAxis[i] : -32767;
214 const int calib_max = m_maxPerAxis.size() > i ? m_maxPerAxis[i] : 32767;
215
216 output.axes[i] =
217 -1.0f + 2.0f * (m_joystate_axes[i] - calib_min) / static_cast<float>(calib_max - calib_min);
218 }
219
220 return true;
221 #else
222 // Apple.
223 return false;
224 #endif
225 MRPT_END
226 }
227
228 void CJoystick::setLimits(const std::vector<int>& minPerAxis, const std::vector<int>& maxPerAxis)
229 {
230 m_minPerAxis = minPerAxis;
231 m_maxPerAxis = maxPerAxis;
232 }