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