vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Analog_5dt.C
Go to the documentation of this file.
1 // This is a driver for the 5dt Data Glove
2 // Look at www.5dt.com for more informations about this product
3 // Manuals are avalaible freely from this site
4 // This code was written by Philippe DAVID with the help of Yves GAUVIN
5 // naming convention used int this file:
6 // l_ is the prefixe for local variables
7 // g_ is the prefixe for global variables
8 // p_ is the prefixe for parameters
9 
10 #include <stdio.h> // for sprintf, printf
11 
12 #include "vrpn_Analog_5dt.h"
13 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
14 #include "vrpn_Serial.h"
15 #include "vrpn_Shared.h" // for timeval, vrpn_SleepMsecs, etc
16 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
17 
18 #undef VERBOSE
19 
20 // Defines the modes in which the device can find itself.
21 #define STATUS_RESETTING (-1) // Resetting the device
22 #define STATUS_SYNCING (0) // Looking for the first character of report
23 #define STATUS_READING (1) // Looking for the rest of the report
24 
25 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
26 
27 
28 /******************************************************************************
29  * NAME : vrpn_5DT::vrpn_5DT
30  * ROLE : This creates a vrpn_5DT and sets it to reset mode. It opens
31  * the serial device using the code in the vrpn_Serial_Analog constructor.
32  * ARGUMENTS :
33  * RETURN :
34  ******************************************************************************/
35 vrpn_5dt::vrpn_5dt (const char * p_name, vrpn_Connection * p_c, const char * p_port, int p_baud, int p_mode, bool tenbytes):
36  vrpn_Serial_Analog (p_name, p_c, p_port, p_baud, 8, vrpn_SER_PARITY_NONE),
37  _announced(false), // Not yet announced our warning.
38  _numchannels (8), // This is an estimate; will change when reports come
39  _tenbytes (tenbytes), // Do we expect ten-byte messages?
40  _wireless (p_baud == 9600), // 9600 baud implies a wireless glove.
41  _gotInfo (false) // used to know if we have gotten a first full wireless report.
42 {
43  if (_wireless) {
44  // All wireless gloves continually send 10 byte reports and ignore
45  // all requests.
46  _tenbytes = true;
47  p_mode = 2;
48  }
49  // Set the parameters in the parent classes
51 
52  // Set the status of the buttons and analogs to 0 to start
53  clear_values();
54 
55  // Set the mode to reset
57 
58  //Init the mode
59  _mode = p_mode;
60 
61  // Sebastien Kuntz reports the following about the driver they have
62  // written: It works great except for one small detail :
63  // for unknown reasons, there's an extra byte hanging around in the data
64  // stream sent by the 5DT glove in stream mode.
65  // We have the header, finger infos, pitch and roll, checksum and after
66  // that we receive a 0x55 Byte.
67  // The official doc (http://www.5dt.com/downloads/5DTDataGlove5Manual.pdf)
68  // says we should get only 9 bytes, so I don't know if it's a response
69  // from a command somewhere; but the fact is we receive 10 bytes.
70  if (_tenbytes) {
71  _expected_chars = 10;
72  } else {
73  _expected_chars = 9;
74  }
75 }
76 
77 
78 /******************************************************************************
79  * NAME : vrpn_5dt::clear_values
80  * ROLE :
81  * ARGUMENTS :
82  * RETURN :
83  ******************************************************************************/
84 void
86 {
87  int i;
88 
89  for (i = 0; i < _numchannels; i++)
90  {
92  }
93 }
94 
95 
96 /******************************************************************************
97  * NAME : vrpn_5dt::
98  * ROLE :
99  * ARGUMENTS : char *cmd : the command to be sent
100  * int len : Length of the command to be sent
101  * RETURN : 0 on success, -1 on failure.
102  ******************************************************************************/
103 int
104 vrpn_5dt::send_command (const unsigned char *p_cmd, int p_len)
105 {
106  int l_ret;
107 
108  // Send the command
109  l_ret = vrpn_write_characters (serial_fd, p_cmd, p_len);
110  // Tell if this all worked.
111  if (l_ret == p_len) {
112  return 0;
113  } else {
114  return -1;
115  }
116 }
117 
118 
119 /******************************************************************************
120  * NAME : vrpn_5dt::reset
121  * ROLE : This routine will reset the 5DT
122  * ARGUMENTS :
123  * RETURN : 0 else -1 in case of error
124  ******************************************************************************/
125 int
127 {
128  struct timeval l_timeout;
129  unsigned char l_inbuf [45];
130  int l_ret;
131  char l_errmsg[256];
132 
133  if (_wireless) {
134  // Wireless gloves can't be reset, but we do need to wait for a header byte.
135  // Then, syncing will wait to see if we get a capability byte as expected
136  // at the end of a report.
137  _gotInfo = false;
138  vrpn_SleepMsecs (100); //Give it time to respond
139 
140  // Will wait at most 2 seconds
141  l_timeout.tv_sec = 2;
142  l_timeout.tv_usec = 0;
143  l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);
144  if (l_ret != 1) {
145  VRPN_MSG_ERROR ("vrpn_5dt: Unable to read from the glove\n");
146  return -1;
147  }
148  if (l_inbuf[0] == 0x80) {
150  _buffer[0] = l_inbuf[0];
151  _bufcount = 1;
152  vrpn_gettimeofday (&timestamp, NULL); // Set watchdog now
153  VRPN_MSG_INFO ("vrpn_5dt: Got a possible header byte!");
154  return 0;
155  }
156  return 0;
157  }
159  send_command ((unsigned char *) "A", 1); // Command to init the glove
160  vrpn_SleepMsecs (100); //Give it time to respond
161  l_timeout.tv_sec = 2;
162  l_timeout.tv_usec = 0;
163  l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);
164 
165  if (l_ret != 1) {
166  VRPN_MSG_ERROR ("vrpn_5dt: Unable to read from the glove\n");
167  return -1;
168  }
169 
170  if (l_inbuf[0] != 85) {
171  VRPN_MSG_ERROR ("vrpn_5dt: Cannot get response on init command");
172  return -1;
173  } else {
175  send_command ( (unsigned char *) "G", 1); //Command to Query informations from the glove
176  vrpn_SleepMsecs (100); //Give it time to respond
177  l_timeout.tv_sec = 2;
178  l_timeout.tv_usec = 0;
179  l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 32, &l_timeout);
180 
181  if (l_ret != 32) {
182  VRPN_MSG_ERROR ("vrpn_5dt: Cannot get info. from the glove");
183  return -1;
184  }
185  if ( (l_inbuf[0] != 66) || (l_inbuf[1] != 82)) {
186  VRPN_MSG_ERROR ("vrpn_5dt: Cannot get good header on info command");
187  return -1;
188  }
189 
190  sprintf (l_errmsg, "vrpn_5dt: glove \"%s\"version %d.%d\n", &l_inbuf [16], l_inbuf [2], l_inbuf [3]);
191  VRPN_MSG_INFO (l_errmsg);
192 
193  if (l_inbuf[4] | 1) {
194  VRPN_MSG_INFO ("A right glove is ready");
195  } else {
196  VRPN_MSG_INFO ("A left glove is ready");
197  }
198  if (l_inbuf[5] | 16) {
199  VRPN_MSG_INFO ("Pitch and Roll are available");
200  } else {
201  VRPN_MSG_INFO ("Pitch and Roll are not available");
202  }
203  }
204 
205  // If we're in continuous mode, request continuous sends
206  if (_mode == 2) {
207  send_command ( (unsigned char *) "D", 1); // Command to query streaming data from the glove
208  }
209 
210  // We're now entering the syncing mode which send the read command to the glove
212 
213  vrpn_gettimeofday (&timestamp, NULL); // Set watchdog now
214  return 0;
215 }
216 
217 
218 /******************************************************************************
219  * NAME : vrpn_5dt::syncing
220  * ROLE : Send the "C" command to ask for new data from the glove
221  * ARGUMENTS : void
222  * RETURN : void
223  ******************************************************************************/
224 void vrpn_5dt::syncing (void)
225 {
226 
227  if (_wireless) {
228  // For a wireless glove, syncing means we got a header byte and need
229  // to wait for the end of the report to see if we guessed right and
230  // will get a capability byte.
231  int l_ret;
234  if (l_ret == -1) {
235  VRPN_MSG_ERROR ("Error reading the glove");
237  return;
238  }
239  _bufcount += l_ret;
240  if (_bufcount < _expected_chars) { // Not done -- go back for more
241  return;
242  }
243  if (_buffer[_bufcount - 1] == 0x40 || _buffer[_bufcount - 1] == 0x01) {
244  VRPN_MSG_INFO ("Got capability byte as expected - switching into read mode.");
245  _bufcount = 0;
247  } else {
248  VRPN_MSG_WARNING ("Got a header byte, but capability byte not found - resetting.");
250  }
251  return;
252  }
253 
254  switch (_mode)
255  {
256  case 1:
257  send_command ((unsigned char *) "C", 1); // Command to query data from the glove
258  break;
259  case 2:
260  // Nothing to be done here -- continuous mode was requested in the reset.
261  break;
262  default :
263  VRPN_MSG_ERROR ("vrpn_5dt::syncing : internal error : unknown state");
264  printf ("mode %d\n", _mode);
265  break;
266  }
267  _bufcount = 0;
269 }
270 
271 
272 /******************************************************************************
273  * NAME : vrpn_5dt::get_report
274  * ROLE : This function will read characters until it has a full report, then
275  * put that report into analog fields and call the report methods on these.
276  * ARGUMENTS : void
277  * RETURN : void
278  ******************************************************************************/
280 {
281  int l_ret; // Return value from function call to be checked
282 
283  // XXX This should be called when the first character of a report is read.
285 
286  //--------------------------------------------------------------------
287  // Read as many bytes of this report as we can, storing them
288  // in the buffer. We keep track of how many have been read so far
289  // and only try to read the rest.
290  //--------------------------------------------------------------------
291 
294  if (l_ret == -1) {
295  VRPN_MSG_ERROR ("Error reading the glove");
297  return;
298  }
299 #ifdef VERBOSE
300  if (l_ret != 0) printf("... got %d characters (%d total)\n",l_ret, _bufcount);
301 #endif
302 
303  //--------------------------------------------------------------------
304  // The time of the report is the time at which the first character for
305  // the report is read.
306  //--------------------------------------------------------------------
307 
308  if ( (l_ret > 0) && (_bufcount == 0) ) {
310  }
311 
312  //--------------------------------------------------------------------
313  // We keep track of how many characters we have received and keep
314  // going back until we get as many as we expect.
315  //--------------------------------------------------------------------
316 
317  _bufcount += l_ret;
318  if (_bufcount < _expected_chars) { // Not done -- go back for more
319  return;
320  }
321 
322  //--------------------------------------------------------------------
323  // We now have enough characters to make a full report. First check to
324  // make sure that the first one is what we expect.
325 
326  if (_buffer[0] != 128) {
327  VRPN_MSG_WARNING ("Unexpected first character in report, resetting");
329  _bufcount = 0;
330  return;
331  }
332 
333  if (_wireless) {
334  if (_buffer[_bufcount - 1] != 0x40 && _buffer[_bufcount - 1] != 0x01) {
335  // The last byte wasn't a capability byte, so this report is invalid.
336  // Reset!
337  VRPN_MSG_WARNING ("Unexpected last character in report, resetting");
339  _bufcount = 0;
340  return;
341  }
342  }
343 
344 #ifdef VERBOSE
345  printf ("Got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
346 #endif
347 
348  //--------------------------------------------------------------------
349  // Decode the report and store the values in it into the analog values
350  // if appropriate.
351  //--------------------------------------------------------------------
352 
353  channel[1] = _buffer[1] / 255.0; //Thumb
354  channel[2] = _buffer[2] / 255.0;
355  channel[3] = _buffer[3] / 255.0;
356  channel[4] = _buffer[4] / 255.0;
357  channel[5] = _buffer[5] / 255.0; // Pinkie
358  channel[6] = 180 * _buffer[6] / 255.0; // Pitch
359  channel[7] = 180 * _buffer[7] / 255.0; // Roll
360 
361  if (_wireless && !_gotInfo) {
362  _gotInfo = true;
363  // Bit 0 set in the capability byte implies a right-hand glove.
364  if (_buffer[9] == 0x01) {
365  VRPN_MSG_INFO ("A 'wireless-type' right glove is ready and reporting");
366  } else {
367  VRPN_MSG_INFO ("A 'wireless-type' left glove is ready and reporting");
368  }
369  }
370 
371  //--------------------------------------------------------------------
372  // Done with the decoding, send the reports and go back to syncing
373  //--------------------------------------------------------------------
374 
375  report_changes();
376  switch (_mode) {
377  case 1:
379  break;
380  case 2: // Streaming Mode, just go back for the next report.
381  _bufcount = 0;
382  break;
383  default :
384  VRPN_MSG_ERROR ("vrpn_5dt::get_report : internal error : unknown state");
385  break;
386  }
387 }
388 
389 
390 /******************************************************************************
391  * NAME : vrpn_5dt::report_changes
392  * ROLE :
393  * ARGUMENTS :
394  * RETURN : void
395  ******************************************************************************/
396 void vrpn_5dt::report_changes (vrpn_uint32 class_of_service)
397 {
399  vrpn_Analog::report_changes(class_of_service);
400 }
401 
402 
403 /******************************************************************************
404  * NAME : vrpn_5dt::report
405  * ROLE :
406  * ARGUMENTS :
407  * RETURN : void
408  ******************************************************************************/
409 void vrpn_5dt::report (vrpn_uint32 class_of_service)
410 {
412  vrpn_Analog::report(class_of_service);
413 }
414 
415 
416 /******************************************************************************
417  * NAME : vrpn_5dt::mainloop
418  * ROLE : This routine is called each time through the server's main loop. It will
419  * take a course of action depending on the current status of the device,
420  * either trying to reset it or trying to get a reading from it. It will
421  * try to reset the device if no data has come from it for a couple of
422  * seconds
423  * ARGUMENTS :
424  * RETURN : void
425  ******************************************************************************/
427 {
428  char l_errmsg[256];
429 
430  server_mainloop();
431  if (_wireless) {
432  if (!_announced) {
433  VRPN_MSG_INFO ("Will connect to a receive-only 'wireless-type' glove - there may be a few warnings before we succeed.");
434  _announced = true;
435  }
436  }
437  switch (_status)
438  {
439  case STATUS_RESETTING:
440  if (reset()== -1)
441  {
442  VRPN_MSG_ERROR ("vrpn_Analog_5dt: Cannot reset the glove!");
443  }
444  break;
445 
446  case STATUS_SYNCING:
447  syncing ();
448  break;
449 
450  case STATUS_READING:
451  {
452  // It turns out to be important to get the report before checking
453  // to see if it has been too long since the last report. This is
454  // because there is the possibility that some other device running
455  // in the same server may have taken a long time on its last pass
456  // through mainloop(). Trackers that are resetting do this. When
457  // this happens, you can get an infinite loop -- where one tracker
458  // resets and causes the other to timeout, and then it returns the
459  // favor. By checking for the report here, we reset the timestamp
460  // if there is a report ready (ie, if THIS device is still operating).
461  get_report();
462  struct timeval current_time;
463  vrpn_gettimeofday (&current_time, NULL);
464  if (vrpn_TimevalDuration (current_time, timestamp) > MAX_TIME_INTERVAL) {
465  sprintf (l_errmsg, "vrpn_5dt::mainloop: Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
466  current_time.tv_sec,
467  static_cast<long> (current_time.tv_usec),
468  timestamp.tv_sec,
469  static_cast<long> (timestamp.tv_usec));
470  VRPN_MSG_ERROR (l_errmsg);
472  }
473  }
474  break;
475 
476  default:
477  VRPN_MSG_ERROR ("vrpn_5dt::mainloop: Unknown mode (internal error)");
478  break;
479  }
480 }
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report iff changed
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
void syncing(void)
virtual void mainloop()
Called once through each main loop iteration to handle updates.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
unsigned _expected_chars
unsigned _bufcount
#define STATUS_RESETTING
virtual void clear_values(void)
Generic connection class not specific to the transport mechanism.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
Definition: vrpn_Analog.C:94
#define VRPN_MSG_WARNING(msg)
unsigned char _buffer[512]
int send_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns 0 on success, -1 on failure.
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
vrpn_5dt(const char *name, vrpn_Connection *c, const char *port, int baud=19200, int mode=1, bool tenbytes=false)
Constructor.
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
#define STATUS_READING
#define MAX_TIME_INTERVAL
#define VRPN_MSG_INFO(msg)
bool _announced
bool _tenbytes
bool _wireless
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
#define STATUS_SYNCING
virtual int reset(void)
int _numchannels
struct timeval timestamp
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
virtual void get_report(void)
#define VRPN_MSG_ERROR(msg)
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39