MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
module-hid.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/modules/module-hid/module-hid.cc,v 1.11 2017/01/12 14:52:24 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
33 
34 #include <cstdint>
35 #include <unistd.h>
36 #include <sys/types.h>
37 #include <sys/stat.h>
38 #include <fcntl.h>
39 
40 #include "dataman.h"
41 #include "filedrv.h"
42 
43 class JoystickDrive : public FileDrive {
44 private:
45  int m_fd;
46 
47  std::vector<bool> m_b_reset;
49  std::vector<doublereal> m_lc_scale;
51 
53 
54  int m_flags;
55 
56  // TODO: echo? -> StreamDriveEcho
57  // TODO: self-describing?
58 
59  bool get_one(void);
60  void init(void);
61 
62  bool get_fd_flags(void);
63  bool set_fd_blocking(bool bBlocking);
64 
65 public:
66  JoystickDrive(unsigned int uL, const DriveHandler* pDH,
67  const std::string& sFileName,
68  integer nButtons, const std::vector<doublereal>& lc_scale,
69  const std::vector<doublereal>& v0);
70 
71  virtual ~JoystickDrive(void);
72 
73  virtual std::ostream& Restart(std::ostream& out) const;
74 
75  virtual void ServePending(const doublereal& t);
76 };
77 
78 JoystickDrive::JoystickDrive(unsigned int uL, const DriveHandler* pDH,
79  const std::string& sFileName,
80  integer nButtons, const std::vector<doublereal>& lc_scale,
81  const std::vector<doublereal>& v0)
82 : FileDrive(uL, pDH, sFileName, nButtons + lc_scale.size(), v0),
83 m_fd(-1),
84 m_b_reset(nButtons), m_nButtons(nButtons),
85 m_lc_scale(lc_scale), m_nLC(m_lc_scale.size()),
86 m_pdLC(pdVal + 1), m_pdB(m_pdLC + m_nLC)
87 {
88 }
89 
91 {
92  if (m_fd != -1) {
93  close(m_fd);
94  }
95  m_fd = -1;
96 }
97 
98 bool
100 {
101  m_flags = fcntl(m_fd, F_GETFL, 0);
102  if (m_flags == -1) {
103  return false;
104  }
105 
106  return true;
107 }
108 
109 bool
111 {
112  int rc;
113 
114  if (bBlocking) {
115  m_flags &= ~O_NONBLOCK;
116 
117  } else {
118  m_flags |= O_NONBLOCK;
119  }
120 
121  rc = fcntl(m_fd, F_SETFL, m_flags);
122 
123  return (rc != -1);
124 }
125 
126 bool
128 {
129  fd_set readfds;
130  FD_ZERO(&readfds);
131  FD_SET(m_fd, &readfds);
132  struct timeval tv = { 0, 0 };
133  int rc = select(m_fd + 1, &readfds, NULL, NULL, &tv);
134 
135 #ifdef HID_DEBUG
136  std::cerr << "select=" << rc << std::endl;
137 #endif // HID_DEBUG
138 
139  switch (rc) {
140  case -1: {
141  int save_errno = errno;
142  char *err_msg = strerror(save_errno);
143 
144  silent_cout("JoystickDrive(" << uLabel << ", " << sFileName << "): select failed"
145  << " (" << save_errno << ": " << err_msg << ")" << std::endl);
147  }
148 
149  case 0:
150  return true;
151 
152  default:
153  if (!FD_ISSET(m_fd, &readfds)) {
154  silent_cout("JoystickDrive"
155  "(" << sFileName << "): "
156  "socket " << m_fd << " reset"
157  << std::endl);
159  }
160  }
161 
162  // toggle blocking to make sure we get the whole package
163  char buf[8];
164  set_fd_blocking(true);
165  ssize_t n2 = read(m_fd, (void *)&buf[0], sizeof(buf));
166  set_fd_blocking(false);
167  if (n2 == -1) {
168 #ifdef HID_DEBUG
169  int save_errno = errno;
170  std::cerr << "recv=" << save_errno << " " << strerror(save_errno) << std::endl;
171 #endif // HID_DEBUG
172  }
173 
174 #ifdef HID_DEBUG
175  uint32_t cnt = *((uint32_t *)&buf[0]);
176 #endif // HID_DEBUG
177  uint8_t type = (uint8_t)buf[6];
178  uint8_t idx = (uint8_t)buf[7];
179  int16_t value = *((int16_t *)&buf[4]);
180 
181 #ifdef HID_DEBUG
182  std::cerr << "n2=" << n2 << " cnt=" << cnt << " value=" << int(value) << " type=" << unsigned(type) << " idx=" << unsigned(idx) << std::endl;
183 #endif // HID_DEBUG
184 
185  switch (type) {
186  case 1:
187  // buttons
188  if (idx < m_nButtons) {
189  /*
190  * when a button is pressed, an event
191  * with non-zero value is recorded;
192  * when the button is released, an event
193  * with zero value is recorded.
194  *
195  * when we see an event with non-zero value,
196  * we set the value to 1., and clear
197  * the "reset" flag; when we see an event with
198  * zero value, we set the reset flag.
199  *
200  * if the button was not previously set,
201  * and it is pressed between two calls to
202  * ServePending(), at the end it is set
203  * and not scheduled for clear.
204  *
205  * if the button was set, and it is
206  * released between two calls to ServePending(),
207  * it is scheduled for clear.
208  *
209  * if the button is pressed and released
210  * multiple times between two calls,
211  * it remains set to 1. and scheduled for clear.
212  */
213  if (value) {
214  m_pdB[idx] = 1.;
215  m_b_reset[idx] = false;
216 
217  } else {
218  m_b_reset[idx] = true;
219  }
220  }
221  // else ignore
222  break;
223 
224  case 2:
225  // linear controls
226 
227  /*
228  * linear controls vary either between 0 and UINT16_MAX
229  * or between -INT16_MAX and INT16_MAX.
230  *
231  * FIXME: currently, we assume -INT16_MAX and INT16_MAX.
232  *
233  * we simply record the last value, and scale it.
234  */
235  if (idx < m_nLC) {
236  m_pdLC[idx] = (m_lc_scale[idx]*value)/INT16_MAX;
237  }
238  break;
239 
240  default:
241  // ignore
242  break;
243  }
244 
245  return false;
246 }
247 
248 void
250 {
251  m_fd = open(sFileName.c_str(), O_RDONLY);
252  if (m_fd == -1) {
253  int save_errno = errno;
254  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << ")::init(): "
255  "unable to open device <" << sFileName << "> "
256  "(" << save_errno << ": " << strerror(save_errno) << ")"
257  << std::endl);
259  }
260 
261  // this probably contains the description of the channels...
262  integer nB = 0, nLC = 0;
263  uint8_t iBmax = 0, iLCmax = 0;
264  for (int i = 0; i < m_nLC + m_nButtons; i++) {
265  char buf[8];
266  ssize_t n = read(m_fd, (void *)&buf[0], sizeof(buf));
267 
268 #ifdef HID_DEBUG
269  std::cerr << "read idx=" << i << " n=" << n << std::endl;
270 #endif // HID_DEBUG
271 
272  if (n == -1) {
273  int save_errno = errno;
274  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << ")::init(): "
275  "read failed (" << save_errno << ": " << strerror(save_errno) << ")"
276  << std::endl);
278  }
279 
280  uint8_t type = (uint8_t)buf[6];
281  uint8_t idx = (uint8_t)buf[7];
282  // int16_t value = *((int16_t *)&buf[4]);
283  if ((type & 0x7F) == 1) {
284  nB++;
285  iBmax = std::max(iBmax, idx);
286 
287  } else if ((type & 0x7F) == 2) {
288  nLC++;
289  iLCmax = std::max(iLCmax, idx);
290 
291  } else {
292  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << "): "
293  "warning, unknown type " << int(type & 0x7F) << ", ignored" << std::endl); }
294 
295 #ifdef HID_DEBUG
296  std::cerr << " type=" << uint(type) << " idx=" << uint(idx) << " value=" << value << std::endl;
297 #endif // HID_DEBUG
298  }
299 
300  bool bFail(false);
301  if (nB != m_nButtons) {
302  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << "): "
303  "inconsistent number of buttons: expected " << m_nButtons << ", got " << nB << std::endl);
304  bFail = true;
305  }
306 
307  if (iBmax >= m_nButtons) {
308  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << "): "
309  "inconsistent largest button index: expected " << m_nButtons - 1 << ", got " << unsigned(iBmax) << std::endl);
310  bFail = true;
311  }
312 
313  if (nLC != m_nLC) {
314  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << "): "
315  "inconsistent number of linear controls: expected " << m_nLC << ", got " << nLC << std::endl);
316  bFail = true;
317  }
318 
319  if (iLCmax >= m_nLC) {
320  silent_cerr("JoystickDrive(" << uLabel << ", " << sFileName << "): "
321  "inconsistent largest linear control index: expected " << m_nLC - 1 << ", got " << unsigned(iLCmax) << std::endl);
322  bFail = true;
323  }
324 
325  if (bFail) {
326  // TODO: error
327  }
328 
329  get_fd_flags();
330  set_fd_blocking(false);
331 }
332 
333 std::ostream&
334 JoystickDrive::Restart(std::ostream& out) const
335 {
336  return out << "# not implemented yet" << std::endl;
337 }
338 
339 void
341 {
342  if (m_fd == -1) {
343  init();
344  }
345 
346  // reset if needed
347  for (integer idx = 0; idx < m_nButtons; idx++) {
348  if (m_b_reset[idx]) {
349  m_pdB[idx] = 0.;
350  m_b_reset[idx] = false;
351  }
352  }
353 
354  // flush buffer
355  while (!get_one()) { NO_OP; };
356 
357 #ifdef HID_DEBUG
358  std::cerr << "JoystickDrive(" << sFileName << ")" << std::endl;
359  for (int i = 0; i < m_nLC + m_nButtons; i++) {
360  std::cerr << " V[" << i << "]=" << pdVal[i] << std::endl;
361  }
362 #endif // HID_DEBUG
363 }
364 
365 struct JoystickDR : public DriveRead {
366  virtual Drive *
367  Read(unsigned uLabel, const DataManager* pDM, MBDynParser& HP);
368 };
369 
370 Drive *
371 JoystickDR::Read(unsigned uLabel, const DataManager* pDM, MBDynParser& HP)
372 {
373  if (HP.IsKeyWord("help")) {
374  silent_cout(
375 " \n"
376 "Module: joystick \n"
377 "Author: Pierangelo Masarati <pierangelo.masarati@polimi.it> \n"
378 "Organization: Dipartimento di Scienze e Tecnologie Aerospaziali \n"
379 " Politecnico di Milano \n"
380 " http://www.aero.polimi.it/ \n"
381 " \n"
382 " All rights reserved \n"
383 " \n"
384 " file: <label> , joystick , \n"
385 " <file> , \n"
386 " <number_of_buttons> , \n"
387 " <number_of_linear_controls> , \n"
388 " [ scale, <scale_factor> [ , ... ] ] ; \n"
389 " \n"
390 " <file> ::= \"/dev/input/js0\" \n"
391 " \n"
392 " <scale_factor>'s default to 1.0 \n"
393  << std::endl);
394 
395  if (!HP.IsArg()) {
396  /*
397  * Exit quietly if nothing else is provided
398  */
399  throw NoErr(MBDYN_EXCEPT_ARGS);
400  }
401  }
402 
403  const char *s = HP.GetFileName();
404  if (s == 0) {
405  // error
406  }
407  std::string sFileName(s);
408 
409  integer nButtons = HP.GetInt();
410  if (nButtons < 0) {
411  // error
412  }
413 
414  integer nLC = HP.GetInt();
415  if (nLC <= 0) {
416  // error
417  }
418 
419  std::vector<doublereal> lc_scale(nLC);
420  if (HP.IsKeyWord("scale")) {
421  for (integer idx = 0; idx < nLC; idx++) {
422  lc_scale[idx] = HP.GetReal();
423  }
424 
425  } else {
426  for (integer idx = 0; idx < nLC; idx++) {
427  lc_scale[idx] = 1.;
428  }
429  }
430 
431  const std::vector<doublereal> v0;
432 
433  return new JoystickDrive(uLabel, pDM->pGetDrvHdl(), sFileName, nButtons, lc_scale, v0);
434 }
435 
436 extern "C" int
437 module_init(const char *module_name, void *pdm, void *php)
438 {
439 #if 0
440  DataManager *pDM = (DataManager *)pdm;
441  MBDynParser *pHP = (MBDynParser *)php;
442 #endif
443 
444  DriveRead *rf = new JoystickDR;
445 
446  if (!SetDriveData("joystick", rf)) {
447  delete rf;
448 
449  silent_cerr("JoystickDrive: "
450  "module_init(" << module_name << ") "
451  "failed" << std::endl);
452 
453  return -1;
454  }
455 
456  return 0;
457 }
458 
bool SetDriveData(const std::string &s, DriveRead *rf)
Definition: drive_.cc:1274
bool get_one(void)
Definition: module-hid.cc:127
#define MBDYN_EXCEPT_ARGS
Definition: except.h:63
virtual integer GetInt(integer iDefval=0)
Definition: parser.cc:1050
JoystickDrive(unsigned int uL, const DriveHandler *pDH, const std::string &sFileName, integer nButtons, const std::vector< doublereal > &lc_scale, const std::vector< doublereal > &v0)
Definition: module-hid.cc:78
doublereal * m_pdB
Definition: module-hid.cc:52
Definition: drive.h:89
doublereal * m_pdLC
Definition: module-hid.cc:52
std::vector< doublereal > m_lc_scale
Definition: module-hid.cc:49
virtual const char * GetFileName(enum Delims Del=DEFAULTDELIM)
Definition: parsinc.cc:673
const DriveHandler * pGetDrvHdl(void) const
Definition: dataman.h:340
std::string sFileName
Definition: filedrv.h:46
int module_init(const char *module_name, void *pdm, void *php)
This function registers our user defined element for the math parser.
Definition: module-hid.cc:437
#define NO_OP
Definition: myassert.h:74
doublereal * pdVal
Definition: filedrv.h:48
integer m_nLC
Definition: module-hid.cc:50
virtual std::ostream & Restart(std::ostream &out) const
Definition: module-hid.cc:334
virtual bool IsKeyWord(const char *sKeyWord)
Definition: parser.cc:910
virtual Drive * Read(unsigned uLabel, const DataManager *pDM, MBDynParser &HP)
Definition: module-hid.cc:371
unsigned int uLabel
Definition: withlab.h:44
integer m_nButtons
Definition: module-hid.cc:48
Definition: except.h:79
virtual void ServePending(const doublereal &t)
Definition: module-hid.cc:340
bool get_fd_flags(void)
Definition: module-hid.cc:99
virtual bool IsArg(void)
Definition: parser.cc:807
bool set_fd_blocking(bool bBlocking)
Definition: module-hid.cc:110
static const std::vector< doublereal > v0
Definition: fixedstep.cc:45
static doublereal buf[BUFSIZE]
Definition: discctrl.cc:333
virtual ~JoystickDrive(void)
Definition: module-hid.cc:90
double doublereal
Definition: colamd.c:52
long int integer
Definition: colamd.c:51
static std::stack< const HighParser * > pHP
Definition: parser.cc:598
void init(void)
Definition: module-hid.cc:249
static void * read(LoadableElem *pEl, DataManager *pDM, MBDynParser &HP)
std::vector< bool > m_b_reset
Definition: module-hid.cc:47
virtual doublereal GetReal(const doublereal &dDefval=0.0)
Definition: parser.cc:1056