vrpn  07.33
Virtual Reality Peripheral Network
vrpn_YEI_3Space.C
Go to the documentation of this file.
1 // This is a driver for the YEI 3Space serial-port devices.
2 
3 #include <stdio.h> // for sprintf, printf
4 
5 #include "vrpn_YEI_3Space.h"
6 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
7 #include "quat.h"
8 
9 //#define VERBOSE
10 
11 // Defines the modes in which the device can find itself.
12 #define STATUS_NOT_INITIALIZED (-2) // Not yet initialized
13 #define STATUS_RESETTING (-1) // Resetting the device
14 #define STATUS_READING (0) // Looking for the a report
15 
16 // The wired USB connection can stream up to 256 bytes, but the wireless
17 // can stream a maximum of 96 bytes per sensor.
18 static const int REPORT_LENGTH = 16 + 16 + 12 + 36 + 4 + 4 + 1;
19 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
20 
21 
22 /******************************************************************************
23  * NAME : vrpn_YEI_3Space::vrpn_YEI_3Space
24  * ROLE : This creates a vrpn_YEI_3Space and sets it to reset mode.
25  * ARGUMENTS :
26  * RETURN :
27  ******************************************************************************/
29  , vrpn_Connection * p_c
30  , bool calibrate_gyros_on_setup
31  , bool tare_on_setup
32  , double frames_per_second
33  , double red_LED_color
34  , double green_LED_color
35  , double blue_LED_color
36  , int LED_mode
37  , const char *reset_commands[])
38  : vrpn_Tracker_Server (p_name, p_c, 2)
39  , vrpn_Analog (p_name, p_c)
40  , vrpn_Button_Filter(p_name, p_c)
41  , d_frames_per_second(frames_per_second)
42 {
43  // Count the reset commands and allocate an array to store them in.
44  const char **ptr = reset_commands;
45  d_reset_commands = NULL;
47  if (ptr != NULL) while ((*ptr) != NULL) {
49  ptr++;
50  }
51  if (d_reset_command_count > 0) {
53  if (d_reset_commands == NULL) {
54  fprintf(stderr,"vrpn_YEI_3Space::vrpn_YEI_3Space(): Out of memory, ignoring reset commands\n");
56  }
57  }
58 
59  // Copy any reset commands.
60  ptr = reset_commands;
61  for (int i = 0; i < d_reset_command_count; i++) {
62  d_reset_commands[i] = new char[strlen(reset_commands[i]) + 1];
63  if (d_reset_commands[i] == NULL) {
64  fprintf(stderr,"vrpn_YEI_3Space::vrpn_YEI_3Space(): Out of memory, giving up\n");
65  return;
66  }
67  strcpy(d_reset_commands[i], reset_commands[i]);
68  }
69 
70  // Set the parameters in the parent classes
72 
73  // Initialize the state of all the buttons
75  memset(buttons, 0, sizeof(buttons));
76  memset(lastbuttons, 0, sizeof(lastbuttons));
77 
78  // We're constructed, but not yet initialized.
80 }
81 
82 /******************************************************************************
83  * NAME : vrpn_YEI_3Space::~vrpn_YEI_3Space
84  * ROLE : This destroys a vrpn_YEI_3Space and frees its memory.
85  * ARGUMENTS :
86  * RETURN :
87  ******************************************************************************/
89 {
90  // Free the space used to store the additional reset commands,
91  // then free the array used to store the pointers.
92  for (int i = 0; i < d_reset_command_count; i++) {
93  delete [] d_reset_commands[i];
94  }
95  if (d_reset_commands != NULL) {
96  delete [] d_reset_commands;
97  d_reset_commands = NULL;
98  }
99 }
100 
101 /******************************************************************************
102  * NAME : vrpn_YEI_3Space::vrpn_YEI_3Space
103  * ROLE : This creates a vrpn_YEI_3Space and sets it to reset mode.
104  * ARGUMENTS :
105  * RETURN :
106  ******************************************************************************/
107 void vrpn_YEI_3Space::init (bool calibrate_gyros_on_setup
108  , bool tare_on_setup
109  , double frames_per_second
110  , double red_LED_color
111  , double green_LED_color
112  , double blue_LED_color
113  , int LED_mode)
114 {
115  // Configure LED mode.
116  unsigned char set_LED_mode[2] = { 0xC4, 0 };
117  set_LED_mode[1] = static_cast<unsigned char>(LED_mode);
118  if (!send_binary_command(set_LED_mode, sizeof(set_LED_mode))) {
119  VRPN_MSG_ERROR ("vrpn_YEI_3Space::init: Unable to send set-LED-mode command\n");
120  }
121 #ifdef VERBOSE
122  printf("LED mode set\n");
123 #endif
124 
125  // Configure LED color.
126  unsigned char set_LED_color[13] = { 0xEE, 0,0,0,0,0,0,0,0,0,0,0,0 };
127  unsigned char *bufptr = &set_LED_color[1];
128  vrpn_int32 buflen = 12;
129  vrpn_float32 LEDs[3];
130  LEDs[0] = static_cast<vrpn_float32>(red_LED_color);
131  LEDs[1] = static_cast<vrpn_float32>(green_LED_color);
132  LEDs[2] = static_cast<vrpn_float32>(blue_LED_color);
133  vrpn_buffer(&bufptr, &buflen, LEDs[0]);
134  vrpn_buffer(&bufptr, &buflen, LEDs[1]);
135  vrpn_buffer(&bufptr, &buflen, LEDs[2]);
136  if (!send_binary_command (set_LED_color, sizeof(set_LED_color))) {
137  VRPN_MSG_ERROR ("vrpn_YEI_3Space::init: Unable to send set-LED-color command\n");
138  }
139 
140  // If we're supposed to calibrate the gyros on startup, do so now.
141  if (calibrate_gyros_on_setup) {
142  unsigned char begin_gyroscope_calibration[1] = { 0xA5 };
143  if (!send_binary_command (begin_gyroscope_calibration, sizeof(begin_gyroscope_calibration))) {
144  VRPN_MSG_ERROR ("vrpn_YEI_3Space::init: Unable to send set-gyroscope-calibration command\n");
145  }
146  }
147 
148  // If we're supposed to tare on startup, do so now.
149  if (tare_on_setup) {
150  unsigned char tare[1] = { 0x60 };
151  if (!send_binary_command (tare, sizeof(tare))) {
152  VRPN_MSG_ERROR ("vrpn_YEI_3Space::init: Unable to send tare command\n");
153  }
154  }
155 
156  // Set the mode to reset
159 #ifdef VERBOSE
160  printf("init() complete\n");
161 #endif
162 }
163 
164 /******************************************************************************
165  * NAME : vrpn_YEI_3Space::reset
166  * ROLE : This routine will reset the YEI_3Space
167  * ARGUMENTS :
168  * RETURN : 0 else -1 in case of error
169  ******************************************************************************/
171 {
172  struct timeval l_timeout;
173 
174  // Ask the device to stop streaming and then wait a little and flush the
175  // input buffer and then ask it for the LED mode and make sure we get a response.
176  unsigned char stop_streaming[1] = { 0x56 };
177  if (!send_binary_command (stop_streaming, sizeof(stop_streaming))) {
178  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send stop-streaming command\n");
179  return -1;
180  }
181  vrpn_SleepMsecs(50);
182  flush_input();
183  unsigned char get_led_mode[1] = { 0xC8 };
184  if (!send_binary_command (get_led_mode, sizeof(get_led_mode))) {
185  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send get-led-mode command\n");
186  return -1;
187  }
188  l_timeout.tv_sec = 2;
189  l_timeout.tv_usec = 0;
190  if (!receive_LED_mode_response(&l_timeout)) {
191  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
192  return -1;
193  }
194 
195  // Request the 3 LED colors and set our internal values to match them.
196  unsigned char get_led_values[1] = { 0xEF };
197  if (!send_binary_command (get_led_values, sizeof(get_led_values))) {
198  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send get-led-values command\n");
199  return -1;
200  }
201  if (!receive_LED_values_response(&l_timeout)) {
202  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to read get-led-mode response\n");
203  return -1;
204  }
205 
206  // Change into the x-right, z-up right handed CS we want
207  unsigned char set_rh_system[] = { 0x74, 0x01 };
208  if (!send_binary_command(set_rh_system, sizeof(set_rh_system))) {
209  VRPN_MSG_ERROR("vrpn_YEI_3Space::reset: Unable to send coordinate system selection command\n");
210  return -1;
211  }
212 
213  // Configure streaming speed based on the requested frames/second value.
214  unsigned char set_streaming_timing[13] = { 0x52, 0,0,0,0,0,0,0,0,0,0,0,0 };
215  vrpn_uint32 interval;
216  if (d_frames_per_second <= 0) {
217  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Bad frames/second value, setting to maximum\n");
218  interval = 0;
219  } else {
220  interval = static_cast<vrpn_uint32>(1e6 * 1 / d_frames_per_second);
221  }
222  vrpn_uint32 duration = 0xFFFFFFFF;
223  vrpn_uint32 delay = 0;
224  unsigned char *bufptr = &set_streaming_timing[1];
225  vrpn_int32 buflen = 12;
226  vrpn_buffer(&bufptr, &buflen, interval);
227  vrpn_buffer(&bufptr, &buflen, duration);
228  vrpn_buffer(&bufptr, &buflen, delay);
229  if (!send_binary_command (set_streaming_timing, sizeof(set_streaming_timing))) {
230  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send set-streaming-timing command\n");
231  return -1;
232  }
233 
234  // Configure the set of values we want to have streamed.
235  // The value 0xFF means "nothing." We want to stream the untared
236  // orientiation as a quaternion, the tared orientation as a quaternion,
237  // all corrected sensor data, the temperature in Celsius, and the
238  // confidence factor.
239  unsigned char set_streaming_slots[9] = { 0x50,
240  0x06, // untared quat
241  0x00, // tared quat
242  0x29, // tared corrected linear acceleration with gravity removed
243  0x25, // all corrected sensor data (3D vectors: rate gyro in rad/s, accel in g, and compass in gauss)
244  0x2B, // temperature C
245  0x2D, // confidence factor
246  0xFA, // button state
247  0xFF }; // followed by empty streaming spots.
248  if (!send_binary_command (set_streaming_slots, sizeof(set_streaming_slots))) {
249  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send set-streaming-slots command\n");
250  return -1;
251  }
252 
253  // Send any additional ACII reset commands that were passed into the
254  // constructor.
255  for (int i = 0; i < d_reset_command_count; i++) {
257  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send additional reset command\n");
258  return -1;
259  }
260  }
261 
262  // Start streaming mode.
263  unsigned char start_streaming[1] = { 0x55 };
264  if (!send_binary_command (start_streaming, sizeof(start_streaming))) {
265  VRPN_MSG_ERROR ("vrpn_YEI_3Space::reset: Unable to send start-streaming command\n");
266  return -1;
267  }
268 
269  // We're now entering the reading mode with no characters.
270  d_expected_characters = REPORT_LENGTH;
271  d_characters_read = 0;
273 
274  vrpn_gettimeofday (&timestamp, NULL); // Set watchdog now
275  return 0;
276 }
277 
278 /******************************************************************************
279  * NAME : vrpn_YEI_3Space::handle_report
280  * ROLE : This function will parse a full report, then
281  * put that report into analog fields and call the report methods on these.
282  * ARGUMENTS : void
283  * RETURN : void
284  ******************************************************************************/
285 void vrpn_YEI_3Space::handle_report(unsigned char *report)
286 {
287  vrpn_float32 value;
288  unsigned char *bufptr = report;
289 
290  // Read the two orientations and report them
291  q_vec_type pos;
292  pos[Q_X] = pos[Q_Y] = pos[Q_Z] = 0;
293  q_type quat;
294 
295  for (int i = 0; i < 2; i++) {
296  vrpn_unbuffer(&bufptr, &value);
297  quat[Q_X] = value;
298  vrpn_unbuffer(&bufptr, &value);
299  quat[Q_Y] = value;
300  vrpn_unbuffer(&bufptr, &value);
301  quat[Q_Z] = value;
302  vrpn_unbuffer(&bufptr, &value);
303  quat[Q_W] = value;
304  if (0 != report_pose(i, timestamp, pos, quat)) {
305  VRPN_MSG_ERROR ("vrpn_YEI_3Space::handle_report(): Error sending sensor report");
307  }
308  }
309 
310  // XXX Linear rate gyros into orientation change?
311 
312  // Read the three values for linear acceleration in tared
313  // space with gravity removed. Convert it into units of
314  // meters/second/second. Put it into the tared sensor.
315  q_vec_type acc;
316  q_type acc_quat;
317  acc_quat[Q_X] = acc_quat[Q_Y] = acc_quat[Q_Z] = 0; acc_quat[Q_W] = 1;
318  vrpn_unbuffer(&bufptr, &value);
319  static const double GRAVITY = 9.80665; // Meters/second/second
320  acc[Q_X] = value * GRAVITY;
321  vrpn_unbuffer(&bufptr, &value);
322  acc[Q_Y] = value * GRAVITY;
323  vrpn_unbuffer(&bufptr, &value);
324  acc[Q_Z] = value * GRAVITY;
325  int sensor = 1;
326  double interval = 1;
327  if (0 != report_pose_acceleration(sensor, timestamp, acc, acc_quat, interval)) {
328  VRPN_MSG_ERROR ("vrpn_YEI_3Space::handle_report(): Error sending acceleration report");
330  }
331 
332  // Read the analog values and put them into the channels.
333  for (int i = 0; i < vrpn_Analog::getNumChannels(); i++) {
334  vrpn_unbuffer(&bufptr, &value);
335  channel[i] = value;
336  }
337 
338  // Check the temperature to make sure it is above 1.
339  // If not, there is trouble parsing the report. Sometimes the wired
340  // unit gets the wrong number of bytes in the report, causing things
341  // to wrap around. This catches that case.
342  if (channel[9] <= 1) {
343  VRPN_MSG_ERROR("vrpn_YEI_3Space::handle_report(): Invalid temperature, resetting");
345  }
346 
347  // Check the confidence factor to make sure it is between 0 and 1.
348  // If not, there is trouble parsing the report. Sometimes the wired
349  // unit gets the wrong number of bytes in the report, causing things
350  // to wrap around. This catches that case.
351  if ((channel[10] < 0) || (channel[10] > 1)) {
352  VRPN_MSG_ERROR("vrpn_YEI_3Space::handle_report(): Invalid confidence, resetting");
354  }
355 
356  // Read the button values and put them into the buttons.
357  vrpn_uint8 b;
358  vrpn_unbuffer(&bufptr, &b);
359  for (int i = 0; i < 8; i++) {
360  buttons[i] = (b & (1 << i)) != 0;
361  }
362 
363  //--------------------------------------------------------------------
364  // Done with the decoding, send the analog and button reports.
365  //--------------------------------------------------------------------
366 
367  report_changes();
368 }
369 
370 /******************************************************************************
371  * NAME : vrpn_YEI_3Space::report_changes
372  * ROLE :
373  * ARGUMENTS :
374  * RETURN : void
375  ******************************************************************************/
376 void vrpn_YEI_3Space::report_changes (vrpn_uint32 class_of_service)
377 {
379  vrpn_Analog::report_changes(class_of_service);
382 }
383 
384 
385 /******************************************************************************
386  * NAME : vrpn_YEI_3Space::report
387  * ROLE :
388  * ARGUMENTS :
389  * RETURN : void
390  ******************************************************************************/
391 void vrpn_YEI_3Space::report (vrpn_uint32 class_of_service)
392 {
394  vrpn_Analog::report(class_of_service);
395 }
396 
397 /******************************************************************************
398  * NAME : vrpn_YEI_3Space::mainloop
399  * ROLE : This routine is called each time through the server's main loop. It will
400  * take a course of action depending on the current status of the device,
401  * either trying to reset it or trying to get a reading from it. It will
402  * try to reset the device if no data has come from it for a couple of
403  * seconds
404  * ARGUMENTS :
405  * RETURN : void
406  ******************************************************************************/
408 {
409  char l_errmsg[256];
410 
411  server_mainloop();
412 
413  switch (d_status)
414  {
416  break;
417 
418  case STATUS_RESETTING:
419  if (reset()== -1) {
420  VRPN_MSG_ERROR ("vrpn_YEI_3Space: Cannot reset!");
421  }
422  break;
423 
424  case STATUS_READING:
425  {
426  // It turns out to be important to get the report before checking
427  // to see if it has been too long since the last report. This is
428  // because there is the possibility that some other device running
429  // in the same server may have taken a long time on its last pass
430  // through mainloop(). Trackers that are resetting do this. When
431  // this happens, you can get an infinite loop -- where one tracker
432  // resets and causes the other to timeout, and then it returns the
433  // favor. By checking for the report here, we reset the timestamp
434  // if there is a report ready (ie, if THIS device is still operating).
435  while (get_report()) {}
436  struct timeval current_time;
437  vrpn_gettimeofday (&current_time, NULL);
438  if (vrpn_TimevalDuration (current_time, timestamp) > MAX_TIME_INTERVAL) {
439  sprintf (l_errmsg, "vrpn_YEI_3Space::mainloop: Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
440  current_time.tv_sec,
441  static_cast<long> (current_time.tv_usec),
442  timestamp.tv_sec,
443  static_cast<long> (timestamp.tv_usec));
444  VRPN_MSG_ERROR (l_errmsg);
446  }
447  }
448  break;
449 
450  default:
451  VRPN_MSG_ERROR ("vrpn_YEI_3Space::mainloop: Unknown mode (internal error)");
452  break;
453  }
454 }
455 
456 
457 /******************************************************************************
458  * NAME : vrpn_YEI_3Space_Sensor::vrpn_YEI_3Space_Sensor
459  * ROLE : This creates a vrpn_YEI_3Space_Sensor and sets it to reset mode. It opens
460  * the serial port to use to communicate to it.
461  * ARGUMENTS :
462  * RETURN :
463  ******************************************************************************/
465  , vrpn_Connection * p_c
466  , const char * p_port
467  , int p_baud
468  , bool calibrate_gyros_on_setup
469  , bool tare_on_setup
470  , double frames_per_second
471  , double red_LED_color
472  , double green_LED_color
473  , double blue_LED_color
474  , int LED_mode
475  , const char *reset_commands[])
476  : vrpn_YEI_3Space (p_name, p_c, calibrate_gyros_on_setup
477  , tare_on_setup, frames_per_second, red_LED_color
478  , green_LED_color, blue_LED_color, LED_mode
479  , reset_commands)
480 {
481  // Open the serial port we're going to use to talk with the device.
482  if ((d_serial_fd = vrpn_open_commport(p_port, p_baud,
483  8, vrpn_SER_PARITY_NONE)) == -1) {
484  perror("vrpn_YEI_3Space_Sensor::vrpn_YEI_3Space_Sensor: Cannot open serial port");
485  fprintf(stderr," (port %s)\n", p_port);
486  }
487 
488  // Initialize the state of the device, now that we've established a
489  // way to talk with it.
490  init(calibrate_gyros_on_setup
491  , tare_on_setup, frames_per_second, red_LED_color
492  , green_LED_color, blue_LED_color, LED_mode);
493 }
494 
495 /******************************************************************************
496  * NAME : vrpn_YEI_3Space_Sensor::~vrpn_YEI_3Space_Sensor
497  * ROLE : This destroys a vrpn_YEI_3Space_Sensor and closes its ports.
498  * ARGUMENTS :
499  * RETURN :
500  ******************************************************************************/
502 {
503  // Ask the device to stop streaming.
504  unsigned char stop_streaming[1] = { 0x56 };
505  if (!send_binary_command(stop_streaming, sizeof(stop_streaming))) {
506  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
507  }
508 
509  // Close com port when destroyed.
510  if (d_serial_fd != -1) {
512  }
513 }
514 
515 /******************************************************************************
516  * NAME : vrpn_YEI_3Space_Sensor::send_binary_command
517  * ROLE :
518  * ARGUMENTS : char *cmd : the command to be sent
519  * int len : Length of the command to be sent
520  * RETURN : true on success, false on failure.
521  ******************************************************************************/
522 
523 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
524 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
525 // Binary-mode command packet format:
526 // 0xF7 (start of packet)
527 // Command byte (command value)
528 // Command data (0 or more bytes of parameters, in big-endian format)
529 // Checksum (sum of all bytes in the packet % 256)
530 bool vrpn_YEI_3Space_Sensor::send_binary_command (const unsigned char *p_cmd, int p_len)
531 {
532  const unsigned char START_OF_PACKET = 0xF7;
533 
534  // Compute the checksum. The description of the checksum implies that it should
535  // include the START_OF_PACKET data, but the example provided in page 28 does not
536  // include that data in the checksum, so we only include the packet data here.
537  int checksum = 0;
538  for (int i = 0; i < p_len; i++) {
539  checksum += p_cmd[i];
540  }
541  checksum %= 256;
542 
543  // Pack the command into the buffer with the start-of-packet and checksum.
544  unsigned char buffer[256]; // Large enough buffer to hold all possible commands.
545  buffer[0] = START_OF_PACKET;
546  memcpy(&(buffer[1]), p_cmd, p_len);
547  buffer[p_len + 1] = static_cast<unsigned char>(checksum);
548 
549  // Send the command and return whether it worked.
550  int l_ret;
551  l_ret = vrpn_write_characters (d_serial_fd, buffer, p_len+2);
552  // Tell if this all worked.
553  if (l_ret == p_len+2) {
554  return true;
555  } else {
556  return false;
557  }
558 }
559 
560 /******************************************************************************
561  * NAME : vrpn_YEI_3Space_Sensor::send_ascii_command
562  * ROLE :
563  * ARGUMENTS : char *cmd : the command to be sent
564  * RETURN : true on success, false on failure.
565  ******************************************************************************/
566 
567 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
568 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
569 // Ascii-mode command packet format:
570 // ':' (start of packet)
571 // Command value (decimal)
572 // ',' Command data (0 or more bytes of parameters
573 // '\n'
575 {
576  // Check to make sure we have a non-empty command
577  if (strlen(p_cmd) == 0) {
578  return false;
579  }
580 
581  // Allocate space for the command plus padding and zero terminator
582  int buflen = static_cast<int>(strlen(p_cmd) + 3);
583  unsigned char *buffer = new unsigned char[buflen];
584  if (buffer == NULL) {
585  return false;
586  }
587 
588  // Fill in the command
589  buffer[0] = ':';
590  memcpy(&buffer[1], p_cmd, strlen(p_cmd));
591  buffer[buflen-2] = '\n';
592  buffer[buflen-1] = 0;
593 
594  // Send the command and see if it worked.
595  int l_ret = vrpn_write_characters (d_serial_fd, buffer, buflen);
596 
597  // Free the buffer.
598  delete [] buffer;
599 
600  // Tell if sending worked.
601  if (l_ret == buflen) {
602  return true;
603  } else {
604  return false;
605  }
606 }
607 
608 /******************************************************************************
609  * NAME : vrpn_YEI_3Space_Sensor::receive_LED_mode_response
610  * ROLE :
611  * ARGUMENTS : struct timeval *timeout; how long to wait, NULL for forever
612  * RETURN : true on success, false on failure.
613  ******************************************************************************/
614 
615 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
616 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
617 // Binary-mode command packet format:
618 // Single byte: value 0 or 1.
620 {
621  unsigned char value;
622  int ret = vrpn_read_available_characters (d_serial_fd, &value, sizeof(value), timeout);
623  if (ret != sizeof(value)) {
624  return false;
625  }
626  d_LED_mode = value;
627  return true;
628 }
629 
630 /******************************************************************************
631  * NAME : vrpn_YEI_3Space_Sensor::receive_LED_values_response
632  * ROLE :
633  * ARGUMENTS : struct timeval *timeout; how long to wait, NULL for forever
634  * RETURN : true on success, false on failure.
635  ******************************************************************************/
636 
637 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
638 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
639 // Binary-mode command packet format:
640 // Three four-byte float responses, each big-endian.
642 {
643  unsigned char buffer[3*sizeof(vrpn_float32)];
644  int ret = vrpn_read_available_characters (d_serial_fd, buffer, sizeof(buffer), timeout);
645  if (ret != sizeof(buffer)) {
646  return false;
647  }
648  vrpn_float32 value;
649  unsigned char *bufptr = buffer;
650  vrpn_unbuffer(&bufptr, &value);
651  d_LED_color[0] = value;
652  vrpn_unbuffer(&bufptr, &value);
653  d_LED_color[1] = value;
654  vrpn_unbuffer(&bufptr, &value);
655  d_LED_color[2] = value;
656  return true;
657 }
658 
659 /******************************************************************************
660  * NAME : vrpn_YEI_3Space_Sensor::get_report
661  * ROLE : This function will read characters until it has a full report, then
662  * call handle_report() with the report and clear the counts.
663  * ARGUMENTS : void
664  * RETURN : bool: Did I get a complete report?
665  ******************************************************************************/
667 {
668  int l_ret; // Return value from function call to be checked
669 
670  //--------------------------------------------------------------------
671  // Read as many bytes of this report as we can, storing them
672  // in the buffer. We keep track of how many have been read so far
673  // and only try to read the rest.
674  //--------------------------------------------------------------------
675 
678  if (l_ret == -1) {
679  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor::get_report(): Error reading the sensor, resetting");
681  return false;
682  }
683 #ifdef VERBOSE
684  if (l_ret != 0) printf("... got %d characters (%d total)\n",l_ret, d_characters_read);
685 #endif
686 
687  //--------------------------------------------------------------------
688  // The time of the report is the time at which the first character for
689  // the report is read.
690  //--------------------------------------------------------------------
691 
692  if ( (l_ret > 0) && (d_characters_read == 0) ) {
694  }
695 
696  //--------------------------------------------------------------------
697  // We keep track of how many characters we have received and keep
698  // going back until we get as many as we expect.
699  //--------------------------------------------------------------------
700 
701  d_characters_read += l_ret;
702  if (d_characters_read < d_expected_characters) { // Not done -- go back for more
703  return false;
704  }
705 
706  //--------------------------------------------------------------------
707  // We now have enough characters to make a full report. Parse each
708  // input in order and check to make sure they work, then send the results.
709  //--------------------------------------------------------------------
711 
712  //--------------------------------------------------------------------
713  // Clear our counts to be ready for the next report
714  //--------------------------------------------------------------------
715  d_expected_characters = REPORT_LENGTH;
716  d_characters_read = 0;
717  return true;
718 }
719 
721 {
723 }
724 
725 
726 /******************************************************************************
727  * NAME : vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless
728  * ROLE : This creates a vrpn_YEI_3Space_Sensor_Wireless and sets it to
729  * reset mode. This constructor is for the first sensor on a
730  * given dongle, so it opens the serial port and configures the
731  * dongle.
732  * ARGUMENTS :
733  * RETURN :
734  ******************************************************************************/
736  , vrpn_Connection * p_c
737  , int logical_id
738  , int serial_number
739  , const char * p_port
740  , int p_baud
741  , bool calibrate_gyros_on_setup
742  , bool tare_on_setup
743  , double frames_per_second
744  , double red_LED_color
745  , double green_LED_color
746  , double blue_LED_color
747  , int LED_mode
748  , const char *reset_commands[])
749  : vrpn_YEI_3Space (p_name, p_c, calibrate_gyros_on_setup
750  , tare_on_setup, frames_per_second, red_LED_color
751  , green_LED_color, blue_LED_color, LED_mode
752  , reset_commands)
753  , d_i_am_first(true)
754  , d_logical_id(-1)
755 {
756  // Open the serial port we're going to use to talk with the device.
757  if ((d_serial_fd = vrpn_open_commport(p_port, p_baud,
758  8, vrpn_SER_PARITY_NONE)) == -1) {
759  perror("vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Cannot open serial port");
760  fprintf(stderr," (port %s)\n", p_port);
761  }
762 #ifdef VERBOSE
763  printf("Serial port opened\n");
764 #endif
765 
766  // Initialize the dongle state, since we are the first device to connect
767  // to it.
768  if (!configure_dongle()){
769  fprintf(stderr,"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not configure dongle\n");
771  d_serial_fd = -1;
772  return;
773  }
774 #ifdef VERBOSE
775  printf("Dongle configured\n");
776 #endif
777 
778  // Set our serial number in the specified logical-ID slot.
779  if (!set_logical_id(static_cast<vrpn_uint8>(logical_id), serial_number)) {
780  fprintf(stderr,"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not set logical ID\n");
781  return;
782  }
783  d_logical_id = logical_id;
784 
785 #ifdef VERBOSE
786  printf("Logical ID set\n");
787 #endif
788  // Initialize the state of the device, now that we've established a
789  // way to talk with it.
790  init(calibrate_gyros_on_setup
791  , tare_on_setup, frames_per_second, red_LED_color
792  , green_LED_color, blue_LED_color, LED_mode);
793 #ifdef VERBOSE
794  printf("Constructor done\n");
795 #endif
796 }
797 
798 /******************************************************************************
799  * NAME : vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless
800  * ROLE : This creates a vrpn_YEI_3Space_Sensor_Wireless and sets it to
801  * reset mode. This constructor is for not the first sensor on a
802  * given dongle.
803  * ARGUMENTS :
804  * RETURN :
805  ******************************************************************************/
807  , vrpn_Connection * p_c
808  , int logical_id
809  , int serial_number
810  , int serial_file_descriptor
811  , bool calibrate_gyros_on_setup
812  , bool tare_on_setup
813  , double frames_per_second
814  , double red_LED_color
815  , double green_LED_color
816  , double blue_LED_color
817  , int LED_mode
818  , const char *reset_commands[])
819  : vrpn_YEI_3Space (p_name, p_c, calibrate_gyros_on_setup
820  , tare_on_setup, frames_per_second, red_LED_color
821  , green_LED_color, blue_LED_color, LED_mode
822  , reset_commands)
823  , d_i_am_first(false)
824  , d_serial_fd(serial_file_descriptor)
825  , d_logical_id(-1)
826 {
827  // Set our serial number in the specified logical-ID slot.
828  if (!set_logical_id(static_cast<vrpn_uint8>(logical_id), serial_number)) {
829  fprintf(stderr,"vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless: Could not set logical ID\n");
830  return;
831  }
832  d_logical_id = logical_id;
833 
834  // Initialize the state of the device, now that we've established a
835  // way to talk with it.
836  init(calibrate_gyros_on_setup
837  , tare_on_setup, frames_per_second, red_LED_color
838  , green_LED_color, blue_LED_color, LED_mode);
839 }
840 
841 /******************************************************************************
842  * NAME : vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless
843  * ROLE : This destroys a vrpn_YEI_3Space_Sensor_Wireless and closes its ports.
844  * ARGUMENTS :
845  * RETURN :
846  ******************************************************************************/
848 {
849  // Ask the device to stop streaming.
850  unsigned char stop_streaming[1] = { 0x56 };
851  if (!send_binary_command(stop_streaming, sizeof(stop_streaming))) {
852  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless: Unable to send stop-streaming command\n");
853  }
854 
855  // Close com port when destroyed, if I am the first device on the dongle.
856  if (d_i_am_first) if (d_serial_fd != -1) {
858  }
859 }
860 
861 /******************************************************************************
862  * NAME : vrpn_YEI_3Space_Sensor_Wireless::configure_dongle
863  * ROLE : * ARGUMENTS :
864  * RETURN : true on success, false on failure.
865  ******************************************************************************/
866 
868 {
869  // Configure the wireless streaming mode to manual flush.
870  unsigned char set_mode[2] = { 0xb0, 0 };
871  if (!send_binary_command_to_dongle(set_mode, sizeof(set_mode))) {
872  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::configure_dongle: Unable to send set-streaming-mode command\n");
873  return false;
874  }
875 
876  return true;
877 }
878 
879 /******************************************************************************
880  * NAME : vrpn_YEI_3Space_Sensor_Wireless::configure_dongle
881  * ROLE :
882  * ARGUMENTS :
883  * RETURN : true on success, false on failure.
884  ******************************************************************************/
885 
886 bool vrpn_YEI_3Space_Sensor_Wireless::set_logical_id(vrpn_uint8 logical_id, int serial_number)
887 {
888  // Configure logical ID.
889  unsigned char set_id[6] = { 0xd1, 0, 0,0,0,0 };
890  unsigned char *bufptr = &set_id[1];
891  vrpn_int32 buflen = 5;
892  vrpn_buffer(&bufptr, &buflen, logical_id);
893  vrpn_buffer(&bufptr, &buflen, serial_number);
894  if (!send_binary_command_to_dongle(set_id, sizeof(set_id))) {
895  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::set_logical_id: Unable to send set-logical-id command\n");
896  return false;
897  }
898 #ifdef VERBOSE
899  printf("... Logical id %d set to serial_number %x\n", logical_id, serial_number);
900 #endif
901 
902  return true;
903 }
904 
905 /******************************************************************************
906  * NAME : vrpn_YEI_3Space_Sensor_Wireless::send_binary_command_to_dongle
907  * ROLE :
908  * ARGUMENTS : char *cmd : the command to be sent
909  * int len : Length of the command to be sent
910  * RETURN : true on success, false on failure.
911  ******************************************************************************/
912 
913 // This is the same as the send_binary_command() from the wired interface.
914 bool vrpn_YEI_3Space_Sensor_Wireless::send_binary_command_to_dongle (const unsigned char *p_cmd, int p_len)
915 {
916  const unsigned char START_OF_PACKET = 0xF7;
917 
918  // Compute the checksum. The description of the checksum implies that it should
919  // include the START_OF_PACKET data, but the example provided in page 28 does not
920  // include that data in the checksum, so we only include the packet data here.
921  int checksum = 0;
922  for (int i = 0; i < p_len; i++) {
923  checksum += p_cmd[i];
924  }
925  checksum %= 256;
926 
927  // Pack the command into the buffer with the start-of-packet and checksum.
928  unsigned char buffer[256]; // Large enough buffer to hold all possible commands.
929  buffer[0] = START_OF_PACKET;
930  memcpy(&(buffer[1]), p_cmd, p_len);
931  buffer[p_len + 1] = static_cast<unsigned char>(checksum);
932 
933  // Send the command and return whether it worked.
934  int l_ret;
935  l_ret = vrpn_write_characters (d_serial_fd, buffer, p_len+2);
936  // Tell if this all worked.
937  if (l_ret == p_len+2) {
938  return true;
939  } else {
940  return false;
941  }
942 }
943 
944 /******************************************************************************
945  * NAME : vrpn_YEI_3Space_Sensor_Wireless::send_binary_command
946  * ROLE :
947  * ARGUMENTS : char *cmd : the command to be sent
948  * int len : Length of the command to be sent
949  * RETURN : true on success, false on failure.
950  ******************************************************************************/
951 
952 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
953 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
954 // Binary-mode wireless command packet format:
955 // 0xF8 (start of packet)
956 // Logical address
957 // Command byte (command value)
958 // Command data (0 or more bytes of parameters, in big-endian format)
959 // Checksum (sum of all bytes in the packet including the address) % 256
960 // NOTE: In the wireless protocol, all commands are acknowledged, so we
961 // wait for the response to make sure the command was properly received.
962 bool vrpn_YEI_3Space_Sensor_Wireless::send_binary_command (const unsigned char *p_cmd, int p_len)
963 {
964  const unsigned char START_OF_PACKET = 0xF8;
965 
966  // Pack the command into the buffer with the start-of-packet and checksum.
967  unsigned char buffer[256]; // Large enough buffer to hold all possible commands.
968  buffer[0] = START_OF_PACKET;
969  buffer[1] = d_logical_id;
970  memcpy(&(buffer[2]), p_cmd, p_len);
971 
972  // Compute the checksum. The description of the checksum implies that it should
973  // include the START_OF_PACKET data, but the example provided in page 25 does not
974  // include that data in the checksum, so we only include the address and
975  // packet data here.
976  int checksum = 0;
977  for (int i = 0; i < p_len+1; i++) {
978  checksum += buffer[1 + i];
979  }
980  checksum %= 256;
981  buffer[p_len + 2] = static_cast<unsigned char>(checksum);
982 
983  // Send the command.
984  int l_ret;
985  l_ret = vrpn_write_characters (d_serial_fd, buffer, p_len+3);
986  if (l_ret != p_len+3) {
987  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Could not send command\n");
988  return false;
989  }
990 #ifdef VERBOSE
991  printf("... packet of length %d sent\n", l_ret);
992 #endif
993 
994  // Listen for a response telling whether the command was received.
995  // We do not listen for the data returned by the command, but only
996  // check the header on the return to make sure the command was a
997  // success. The caller should separately look for any requested
998  // data.
999  struct timeval timeout;
1000  timeout.tv_sec = 0;
1001  timeout.tv_usec = 500000;
1002  int ret = vrpn_read_available_characters(d_serial_fd, buffer, 3, &timeout);
1003  if (ret == 2) {
1004  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Error (%d) from ID %d\n", buffer[0], buffer[1]);
1005  return false;
1006  } else if (ret != 3) {
1007  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Timeout waiting for command status (got %d chars)\n", ret);
1008  return false;
1009  }
1010  if (buffer[0] != 0) {
1011  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Command failed\n");
1012  return false;
1013  }
1014  if (buffer[1] != d_logical_id) {
1015  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_binary_command: Got response for incorrect logical ID\n");
1016  return false;
1017  }
1018 #ifdef VERBOSE
1019  printf("..... send succeded\n");
1020 #endif
1021 
1022  return true;
1023 }
1024 
1025 /******************************************************************************
1026  * NAME : vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command
1027  * ROLE :
1028  * ARGUMENTS : char *cmd : the command to be sent
1029  * RETURN : true on success, false on failure.
1030  ******************************************************************************/
1031 
1032 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
1033 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
1034 // Ascii-mode command packet format:
1035 // '>' (start of packet)
1036 // ',' and then logical ID (decimal)
1037 // ',' and then data Command value (decimal)
1038 // ',' Command data (0 or more bytes of parameters
1039 // '\n'
1040 // NOTE: In the wireless protocol, all commands are acknowledged, so we
1041 // wait for the response to make sure the command was properly received.
1043 {
1044  // Check to make sure we have a non-empty command
1045  if (strlen(p_cmd) == 0) {
1046  return false;
1047  }
1048 
1049  // Allocate space for the longest command plus padding and zero terminator
1050  char buffer[256];
1051 
1052  // Fill in the command
1053  sprintf(buffer, ">%d,%s\n", d_logical_id, p_cmd);
1054 
1055  // Send the command and see if it worked.
1056  int buflen = static_cast<int>(strlen(buffer) + 1);
1057  unsigned char *bufptr = static_cast<unsigned char *>(
1058  static_cast<void*>(buffer));
1059  int l_ret = vrpn_write_characters(d_serial_fd, bufptr, buflen);
1060 
1061  // Tell if sending worked.
1062  if (l_ret != buflen) {
1063  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Error sending command\n");
1064  return false;
1065  }
1066 
1067  // Listen for a response telling whether the command was received.
1068  // We parse the entire ASCII command response, including the \n at
1069  // the end, since we aren't programmatically handling any of these
1070  // in the driver but only letting the user specify them in the
1071  // reset routine.
1072  struct timeval timeout;
1073  timeout.tv_sec = 0;
1074  timeout.tv_usec = 500000;
1075  int ret;
1076  if ((ret = vrpn_read_available_characters(d_serial_fd, bufptr, sizeof(buffer)-1, &timeout)) <= 0) {
1077  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Timeout waiting for command status\n");
1078  return false;
1079  }
1080  buffer[ret] = '\0';
1081  if (buffer[0] != '0') {
1082  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Command failed: response (%s)\n", buffer);
1083  return false;
1084  }
1085  if (buffer[strlen(buffer)-1] != '\n') {
1086  fprintf(stderr, "vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command: Got ill-formatted response: (%s).\n", buffer);
1087  return false;
1088  }
1089 
1090  return true;
1091 }
1092 
1093 /******************************************************************************
1094  * NAME : vrpn_YEI_3Space_Sensor_Wireless::receive_LED_mode_response
1095  * ROLE :
1096  * ARGUMENTS : struct timeval *timeout; how long to wait, NULL for forever
1097  * RETURN : true on success, false on failure.
1098  ******************************************************************************/
1099 
1100 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
1101 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
1102 // Binary-mode command packet format:
1103 // Single byte: value 0 or 1.
1105 {
1106  unsigned char value;
1107  int ret = vrpn_read_available_characters (d_serial_fd, &value, sizeof(value), timeout);
1108  if (ret != sizeof(value)) {
1109  return false;
1110  }
1111  d_LED_mode = value;
1112  return true;
1113 }
1114 
1115 /******************************************************************************
1116  * NAME : vrpn_YEI_3Space_Sensor_Wireless::receive_LED_values_response
1117  * ROLE :
1118  * ARGUMENTS : struct timeval *timeout; how long to wait, NULL for forever
1119  * RETURN : true on success, false on failure.
1120  ******************************************************************************/
1121 
1122 // Info from YEI 3-Space Sensor User's Manual Wireless 2.0 R21 26 March2014
1123 // http://www.yeitechnology.com/sites/default/files/YEI_3-Space_Sensor_Users_Manual_Wireless_2.0_r21_26Mar2014.pdf
1124 // Binary-mode command packet format:
1125 // Three four-byte float responses, each big-endian.
1127 {
1128  unsigned char buffer[3*sizeof(vrpn_float32)];
1129  int ret = vrpn_read_available_characters (d_serial_fd, buffer, sizeof(buffer), timeout);
1130  if (ret != sizeof(buffer)) {
1131  return false;
1132  }
1133  vrpn_float32 value;
1134  unsigned char *bufptr = buffer;
1135  vrpn_unbuffer(&bufptr, &value);
1136  d_LED_color[0] = value;
1137  vrpn_unbuffer(&bufptr, &value);
1138  d_LED_color[1] = value;
1139  vrpn_unbuffer(&bufptr, &value);
1140  d_LED_color[2] = value;
1141  return true;
1142 }
1143 
1144 // XXXX Fix things below here.
1145 
1146 /******************************************************************************
1147  * NAME : vrpn_YEI_3Space_Sensor_Wireless::get_report
1148  * ROLE : This function will read characters until it has a full report, then
1149  * call handle_report() with the report and clear the counts. For the
1150  * wireless protocol, we need to manually request release of streaming
1151  * characters from our logical ID before checking to see if there are
1152  * any.
1153  * ARGUMENTS : void
1154  * RETURN : bool: Did we get a complete report?
1155  ******************************************************************************/
1157 {
1158  int l_ret; // Return value from function call to be checked
1159 
1160  //--------------------------------------------------------------------
1161  // Request release of pending reports from our ID.
1162  unsigned char release_report[2] = { 0xB4, 0 };
1163  release_report[1] = d_logical_id;
1164  if (!send_binary_command_to_dongle(release_report, sizeof(release_report))) {
1165  VRPN_MSG_ERROR ("vrpn_YEI_3Space::get_report: Unable to send release-report command\n");
1166  return false;
1167  }
1168 
1169  //--------------------------------------------------------------------
1170  // Read a report if it is available.
1171  struct timeval timeout;
1172  timeout.tv_sec = 0;
1173  timeout.tv_usec = 1000;
1175  REPORT_LENGTH + 3,
1176  &timeout);
1177  if (l_ret == -1) {
1178  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reading the sensor, resetting");
1180  return false;
1181  }
1182 #ifdef VERBOSE
1183  if (l_ret != 0) printf("... got %d characters\n",l_ret);
1184 #endif
1185  // If we didn't get any reports, we're done and will look again next time.
1186  if (l_ret == 0) {
1187  return false;
1188  }
1189 
1190  // Check to be sure that the length was what we expect, we got no errors
1191  // (first header byte 0) and the response is from the correct device.
1192  if (l_ret != REPORT_LENGTH + 3) {
1193  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::get_report(): Truncated report, resetting");
1195  return false;
1196  }
1197  if (d_buffer[0] != 0) {
1198  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::get_report(): Error reported, resetting");
1200  return false;
1201  }
1202  if (d_buffer[1] != d_logical_id) {
1203  VRPN_MSG_ERROR ("vrpn_YEI_3Space_Sensor_Wireless::get_report(): Report from wrong sensor received, resetting");
1205  return false;
1206  }
1207 
1208  //--------------------------------------------------------------------
1209  // Parse and handle the report.
1210  vrpn_gettimeofday(&timestamp, NULL);
1211  handle_report(&d_buffer[3]);
1212  return true;
1213 }
1214 
1216 {
1218 }
vrpn_YEI_3Space_Sensor_Wireless::configure_dongle
virtual bool configure_dongle(void)
Configure the dongle (called if we are the first one)
Definition: vrpn_YEI_3Space.C:867
vrpn_Tracker::acc
vrpn_float64 acc[3]
Definition: vrpn_Tracker.h:98
vrpn_YEI_3Space
Base class with routines for YEI 3Space units.
Definition: vrpn_YEI_3Space.h:15
STATUS_NOT_INITIALIZED
#define STATUS_NOT_INITIALIZED
Definition: vrpn_YEI_3Space.C:12
vrpn_Tracker_Server::report_pose_acceleration
virtual int report_pose_acceleration(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_float64 interval, const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tracker.C:807
vrpn_YEI_3Space.h
vrpn_YEI_3Space_Sensor_Wireless::receive_LED_values_response
bool receive_LED_values_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
Definition: vrpn_YEI_3Space.C:1126
vrpn_Button::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_YEI_3Space::reset
virtual int reset(void)
Definition: vrpn_YEI_3Space.C:170
vrpn_YEI_3Space::d_frames_per_second
double d_frames_per_second
Definition: vrpn_YEI_3Space.h:77
vrpn_TimevalDuration
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
vrpn_Button_Filter::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:382
vrpn_YEI_3Space_Sensor_Wireless::send_ascii_command
bool send_ascii_command(const char *cmd)
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
Definition: vrpn_YEI_3Space.C:1042
vrpn_Analog::channel
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_YEI_3Space_Sensor_Wireless::set_logical_id
virtual bool set_logical_id(vrpn_uint8 logical_id, vrpn_int32 serial_number)
Insert our serial number into the specified slot.
Definition: vrpn_YEI_3Space.C:886
vrpn_SER_PARITY_NONE
@ vrpn_SER_PARITY_NONE
Definition: vrpn_Serial.h:16
vrpn_YEI_3Space_Sensor_Wireless::d_serial_fd
int d_serial_fd
Definition: vrpn_YEI_3Space.h:268
vrpn_YEI_3Space::flush_input
virtual void flush_input(void)=0
Flush any incoming characters in the communications channel.
vrpn_YEI_3Space::d_reset_command_count
int d_reset_command_count
Definition: vrpn_YEI_3Space.h:69
vrpn_YEI_3Space_Sensor_Wireless::receive_LED_mode_response
bool receive_LED_mode_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
Definition: vrpn_YEI_3Space.C:1104
vrpn_Tracker_Server
Definition: vrpn_Tracker.h:251
vrpn_Analog::report
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
vrpn_YEI_3Space::d_expected_characters
unsigned d_expected_characters
Definition: vrpn_YEI_3Space.h:101
vrpn_Analog
Definition: vrpn_Analog.h:28
STATUS_READING
#define STATUS_READING
Definition: vrpn_YEI_3Space.C:14
vrpn_Tracker_Server::report_pose
virtual int report_pose(const int sensor, const struct timeval t, const vrpn_float64 position[3], const vrpn_float64 quaternion[4], const vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
These functions should be called to report changes in state, once per sensor.
Definition: vrpn_Tracker.C:729
vrpn_Analog::timestamp
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_YEI_3Space_Sensor_Wireless::d_logical_id
vrpn_uint8 d_logical_id
Definition: vrpn_YEI_3Space.h:269
vrpn_YEI_3Space::d_reset_commands
char ** d_reset_commands
Definition: vrpn_YEI_3Space.h:68
vrpn_Button::num_buttons
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_YEI_3Space_Sensor_Wireless::d_i_am_first
bool d_i_am_first
Definition: vrpn_YEI_3Space.h:267
vrpn_YEI_3Space_Sensor::send_binary_command
bool send_binary_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns true on success,...
Definition: vrpn_YEI_3Space.C:530
vrpn_YEI_3Space::vrpn_YEI_3Space
vrpn_YEI_3Space(const char *name, vrpn_Connection *c, bool calibrate_gyros_on_setup=false, bool tare_on_setup=false, double frames_per_second=50, double red_LED_color=0, double green_LED_color=0, double blue_LED_color=0, int LED_mode=1, const char *reset_commands[]=NULL)
Constructor.
Definition: vrpn_YEI_3Space.C:28
vrpn_YEI_3Space_Sensor::d_serial_fd
int d_serial_fd
Definition: vrpn_YEI_3Space.h:154
vrpn_YEI_3Space_Sensor::~vrpn_YEI_3Space_Sensor
virtual ~vrpn_YEI_3Space_Sensor()
Destructor.
Definition: vrpn_YEI_3Space.C:501
vrpn_Button::buttons
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
vrpn_unbuffer
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:312
vrpn_YEI_3Space::d_LED_color
vrpn_float32 d_LED_color[3]
Definition: vrpn_YEI_3Space.h:79
vrpn_Tracker::acc_quat
vrpn_float64 acc_quat[4]
Definition: vrpn_Tracker.h:98
vrpn_YEI_3Space::d_buffer
unsigned char d_buffer[128]
Definition: vrpn_YEI_3Space.h:100
vrpn_flush_input_buffer
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
vrpn_YEI_3Space_Sensor_Wireless::get_report
virtual bool get_report(void)
Get and handle a report from the device if one is available. Return true if one was available,...
Definition: vrpn_YEI_3Space.C:1156
vrpn_SleepMsecs
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
vrpn_YEI_3Space::receive_LED_values_response
virtual bool receive_LED_values_response(struct timeval *timeout=NULL)=0
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
vrpn_YEI_3Space::handle_report
virtual void handle_report(unsigned char *report)
Definition: vrpn_YEI_3Space.C:285
vrpn_YEI_3Space::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition: vrpn_YEI_3Space.C:407
vrpn_YEI_3Space::d_characters_read
unsigned d_characters_read
Definition: vrpn_YEI_3Space.h:102
vrpn_Button::timestamp
struct timeval timestamp
Definition: vrpn_Button.h:48
vrpn_Analog::getNumChannels
vrpn_int32 getNumChannels(void) const
Definition: vrpn_Analog.C:52
STATUS_RESETTING
#define STATUS_RESETTING
Definition: vrpn_YEI_3Space.C:13
vrpn_YEI_3Space_Sensor_Wireless::vrpn_YEI_3Space_Sensor_Wireless
vrpn_YEI_3Space_Sensor_Wireless(const char *name, vrpn_Connection *c, int logical_id, int serial_number, const char *port, int baud=115200, bool calibrate_gyros_on_setup=false, bool tare_on_setup=false, double frames_per_second=50, double red_LED_color=0, double green_LED_color=0, double blue_LED_color=0, int LED_mode=1, const char *reset_commands[]=NULL)
Constructor for the first device, which will open the serial port and configure the dongle.
Definition: vrpn_YEI_3Space.C:735
vrpn_YEI_3Space::send_ascii_command
virtual bool send_ascii_command(const char *cmd)=0
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
vrpn_YEI_3Space::receive_LED_mode_response
virtual bool receive_LED_mode_response(struct timeval *timeout=NULL)=0
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
vrpn_YEI_3Space_Sensor_Wireless::~vrpn_YEI_3Space_Sensor_Wireless
virtual ~vrpn_YEI_3Space_Sensor_Wireless()
Destructor.
Definition: vrpn_YEI_3Space.C:847
vrpn_YEI_3Space_Sensor::receive_LED_mode_response
bool receive_LED_mode_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-state request command. NULL timeout pointer means wait forever....
Definition: vrpn_YEI_3Space.C:619
vrpn_YEI_3Space::d_status
int d_status
Definition: vrpn_YEI_3Space.h:72
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_YEI_3Space::get_report
virtual bool get_report(void)=0
vrpn_Analog::num_channel
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
vrpn_YEI_3Space_Sensor::vrpn_YEI_3Space_Sensor
vrpn_YEI_3Space_Sensor(const char *name, vrpn_Connection *c, const char *port, int baud=115200, bool calibrate_gyros_on_setup=false, bool tare_on_setup=false, double frames_per_second=50, double red_LED_color=0, double green_LED_color=0, double blue_LED_color=0, int LED_mode=1, const char *reset_commands[]=NULL)
Constructor.
Definition: vrpn_YEI_3Space.C:464
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_YEI_3Space::d_LED_mode
int d_LED_mode
Definition: vrpn_YEI_3Space.h:78
vrpn_YEI_3Space_Sensor_Wireless::flush_input
virtual void flush_input(void)
Flush any incoming characters in the communications channel.
Definition: vrpn_YEI_3Space.C:1215
vrpn_Button::lastbuttons
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
vrpn_YEI_3Space_Sensor::receive_LED_values_response
bool receive_LED_values_response(struct timeval *timeout=NULL)
Read and parse the response to an LED-values request command. NULL timeout pointer means wait forever...
Definition: vrpn_YEI_3Space.C:641
vrpn_read_available_characters
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
vrpn_YEI_3Space_Sensor::send_ascii_command
bool send_ascii_command(const char *cmd)
Put a ':' character at the front and ' ' at the end and then send the resulting command as an ASCII c...
Definition: vrpn_YEI_3Space.C:574
vrpn_close_commport
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:345
vrpn_YEI_3Space::timestamp
struct timeval timestamp
Definition: vrpn_YEI_3Space.h:104
vrpn_YEI_3Space_Sensor::flush_input
virtual void flush_input(void)
Flush any incoming characters in the communications channel.
Definition: vrpn_YEI_3Space.C:720
vrpn_YEI_3Space::~vrpn_YEI_3Space
virtual ~vrpn_YEI_3Space()
Destructor.
Definition: vrpn_YEI_3Space.C:88
vrpn_BaseClass::init
virtual int init(void)
Initialize things that the constructor can't. Returns 0 on success, -1 on failure.
Definition: vrpn_BaseClass.C:363
vrpn_MessageMacros.h
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_buffer
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
Utility routine for placing a timeval struct into a buffer that is to be sent as a message.
Definition: vrpn_Shared.C:241
vrpn_YEI_3Space_Sensor_Wireless::send_binary_command
bool send_binary_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns true on success,...
Definition: vrpn_YEI_3Space.C:962
vrpn_write_characters
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
vrpn_open_commport
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition: vrpn_Serial.C:54
MAX_TIME_INTERVAL
#define MAX_TIME_INTERVAL
Definition: vrpn_YEI_3Space.C:19
vrpn_Analog::report_changes
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
vrpn_Tracker::pos
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
vrpn_YEI_3Space::report
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
Definition: vrpn_YEI_3Space.C:391
vrpn_YEI_3Space_Sensor::get_report
virtual bool get_report(void)
Definition: vrpn_YEI_3Space.C:666
vrpn_YEI_3Space_Sensor_Wireless::send_binary_command_to_dongle
bool send_binary_command_to_dongle(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message to the dongle directly (not a wireless c...
Definition: vrpn_YEI_3Space.C:914
VRPN_MSG_ERROR
#define VRPN_MSG_ERROR(msg)
Definition: vrpn_MessageMacros.h:46
vrpn_YEI_3Space::send_binary_command
virtual bool send_binary_command(const unsigned char *cmd, int len)=0
Compute the CRC for the message, append it, and send message. Returns true on success,...
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603
vrpn_Button_Filter
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65