vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tng3.C
Go to the documentation of this file.
1 // vrpn_Tng3.C
2 // This is a driver for an TNG 3 controller.
3 // This box is a serial-line device that allows the user to connect
4 // 8 analog and 8 digital inputs and, read from them over RS-232.
5 //
6 // This code is based on the Tng3 code from pulsar.org
7 
8 #include <math.h> // for floor
9 #include <stdio.h> // for fprintf, stderr, printf
10 
11 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_WARNING, etc
12 #include "vrpn_Serial.h"
13 #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
14 #include "vrpn_Tng3.h"
15 
16 #define VRPN_TIMESTAMP_MEMBER _timestamp // Configuration required for vrpn_MessageMacros in this class.
17 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
18 
19 #undef VERBOSE
20 
21 #define MAX_TCHANNELS 8
22 #define MAX_TBUTTONS 8
23 
24 #define PAUSE_RESET .015
25 #define PAUSE_END .015
26 #define PAUSE_RESTORE 2.0
27 #define PAUSE_BYTE .015
28 
29 #define DATA_RECORD_LENGTH 9 // 9 bytes follow the start byte
30 
31 
32 // Defines the modes in which the box can find itself.
33 #define STATUS_RESETTING (-1) // Resetting the box
34 #define STATUS_SYNCING (0) // Looking for the first character of report
35 #define STATUS_READING (1) // Looking for the rest of the report
36 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
37 
38 
39 static void pause (double delay) {
40  if (delay < 0)
41  delay = 0;
42  unsigned long interval = (long) floor(1000000.0 * delay);
43 
44  struct timeval start, now;
45  vrpn_gettimeofday (&start, NULL);
46 
47  do {
48  vrpn_gettimeofday (&now, NULL);
49  } while (vrpn_TimevalDuration(now, start) < interval);
50 
51 }
52 
53 // This creates a vrpn_Tng3 and sets it to reset mode. It opens
54 // the serial device using the code in the vrpn_Serial_Analog constructor.
55 // The box will autodetect the baud rate when the "IMMC" command is sent
56 // to it.
57 
58 vrpn_Tng3::vrpn_Tng3 (const char * name,
59  vrpn_Connection * c,
60  const char * port,
61  int baud,
62  const int numbuttons,
63  const int numchannels):
64  vrpn_Serial_Analog(name, c, port, baud),
65  vrpn_Button_Filter(name, c),
66  _numbuttons(numbuttons),
67  _numchannels(numchannels),
68  _num_read(0)
69 {
70  // Verify the validity of the parameters
71  if (_numbuttons > MAX_TBUTTONS) {
72  fprintf(stderr,"vrpn_Tng3: Can only support %d buttons, not %d\n", (int)MAX_TBUTTONS, (int)_numbuttons);
74  }
76  fprintf(stderr,"vrpn_Tng3: Can only support %d analog channels, not %d\n", (int)MAX_TCHANNELS, (int)_numchannels);
78  }
79 
80  // Set the parameters in the parent classes
83 
84  // Set the status of the buttons, analogs and encoders to 0 to start
85  clear_values();
86 
87  // Set the mode to reset
89 }
90 
92 {
93  int i;
94 
95  for (i = 0; i < _numbuttons; i++) {
97  }
98  for (i = 0; i < _numchannels; i++) {
100  }
101 }
102 
103 // This routine will reset the Tng3, asking it to send the requested number
104 // of analogs, buttons and encoders.
105 // It verifies that it can communicate with the device and checks the version of
106 // the device.
107 
109 {
110 
111  //-----------------------------------------------------------------------
112  // Set the values back to zero for all buttons and analogs
113  clear_values();
114 
115  //-----------------------------------------------------------------------
116  // sending an end at this time will force the ibox into the reset mode, if it
117  // was not already. if the box is in the power up mode, nothing will happen because
118  // it 'should' be waiting to sync up the baudrate
119 
120  // try to synchronize for 2 seconds
121 
122  bDataPacketStart = 0x55;
123 
124  if (syncDatastream (2.0)) {
125  printf("vrpn_Tng3 found\n");
126  } else {
127  return -1;
128  }
129 
130  printf("TNG3B found\n");
131 
133  vrpn_gettimeofday(&_timestamp, NULL); // Set watchdog now
134  return 0;
135 }
136 
137 // This function will read characters until it has a full report, then
138 // put that report into the time, analog amd button fields and call
139 // the report methods on these. The time stored is that of
140 // the first character received as part of the report. Each time
141 // through, it gets however many characters are available.
142 //
143 // Reports start with the byte bDataPacketStart followed by
144 // DATA_RECORD_LENGTH bytes
145 //
146 // If we get a report that is not valid, we assume that we have lost a
147 // character or something and re-synchronize by flushing the buffer
148 //
149 // The routine that calls this one
150 // makes sure we get a full reading often enough (ie, it is responsible
151 // for doing the watchdog timing to make sure the box hasn't simply
152 // stopped sending characters).
153 
155 {
156  int i;
157  unsigned int buttonBits = 0;
158 
159  // Zero timeout, poll for any available characters
160  struct timeval timeout = {0, 0};
161 
162  // Go looking for a synchronization byte. This apparently starts out
163  // as 0x55 for one record, then toggles all of the bits for the next
164  // record.
165  if (status == STATUS_SYNCING) {
166  if (1 == vrpn_read_available_characters(serial_fd, _buffer, 1, &timeout)) {
167  // if not a record start, we need to resync
168  if (_buffer[0] != bDataPacketStart) {
169  VRPN_MSG_WARNING("Resyncing");
170  return 0;;
171  }
172 
173  // we got a good start byte... we're reading now
174  _num_read = 0; //< Ignore the status byte for the following record
176 
177  // invert the bits for the next packet start
178  bDataPacketStart ^= 0xFF;
179  }
180  }
181 
182  // we broke out.. if we're not reading, then we have nothing to do
183  if (STATUS_READING != status) {
184  return 0;
185  }
186 
187  // we're reading now, get the report
188 
189  // get the expected number of data record bytes
190  timeout.tv_sec = 0;
191  timeout.tv_usec = 0;
194 
195  if (result < 0) {
196  VRPN_MSG_WARNING("Bad read");
198  return 0;
199  }
200 
201  // If we don't have a full record, go back again.
202  _num_read += result;
204  return 0;
205  }
206 
207  // parse the report here -- we got a full record.
208 
209  // here is where we decode the analog stuff
210  for (i = 0; i < _numchannels; i++) {
212  vrpn_Analog::channel[i] = (unsigned short) _buffer[i];
213  }
214 
215  // get the button bits and make sense of them
216 
217  buttonBits = _buffer[DATA_RECORD_LENGTH-1];
218  for (i = 0; i < _numbuttons; i++) {
220  vrpn_Button::buttons[i] = static_cast<unsigned char>((buttonBits & (1 << i)) ? VRPN_BUTTON_OFF : VRPN_BUTTON_ON);
221  }
222 
223  report_changes();
224  vrpn_gettimeofday(&_timestamp, NULL); // Set watchdog now
225 
227  return 1;
228 }
229 
230 void vrpn_Tng3::report_changes(vrpn_uint32 class_of_service)
231 {
234 
235  vrpn_Analog::report_changes(class_of_service);
237 }
238 
239 void vrpn_Tng3::report(vrpn_uint32 class_of_service)
240 {
243 
244  vrpn_Analog::report(class_of_service);
246 }
247 
248 // This routine is called each time through the server's main loop. It will
249 // take a course of action depending on the current status of the TNG3,
250 // either trying to reset it or trying to get a reading from it.
252 {
253  server_mainloop();
254 
255  switch(status) {
256  case STATUS_RESETTING:
257  reset();
258  break;
259 
260  case STATUS_SYNCING:
261  case STATUS_READING:
262  {
263  // It turns out to be important to get the report before checking
264  // to see if it has been too long since the last report. This is
265  // because there is the possibility that some other device running
266  // in the same server may have taken a long time on its last pass
267  // through mainloop(). Trackers that are resetting do this. When
268  // this happens, you can get an infinite loop -- where one tracker
269  // resets and causes the other to timeout, and then it returns the
270  // favor. By checking for the report here, we reset the timestamp
271  // if there is a report ready (ie, if THIS device is still operating).
272  while (get_report()) {}; // Keep getting reports as long as they come
273  struct timeval current_time;
274  vrpn_gettimeofday(&current_time, NULL);
275  if ( vrpn_TimevalDuration(current_time,_timestamp) > MAX_TIME_INTERVAL) {
276  fprintf(stderr,"TNG3 failed to read... current_time=%ld:%ld, timestamp=%ld:%ld\n",
277  current_time.tv_sec, static_cast<long>(current_time.tv_usec),
278  _timestamp.tv_sec, static_cast<long>(_timestamp.tv_usec));
279  send_text_message("Too long since last report, resetting", current_time, vrpn_TEXT_ERROR);
281  }
282  }
283  break;
284 
285  default:
286  fprintf(stderr,"vrpn_Tng3: Unknown mode (internal error)\n");
287  break;
288  }
289 }
290 
291 
292 // synchronize the data stream
293 // seconds determines how long the process is permitted to continue
294 
295 int vrpn_Tng3::syncDatastream (double seconds) {
296 
297  struct timeval miniDelay;
298  miniDelay.tv_sec = 0;
299  miniDelay.tv_usec = 50000;
300 
301  unsigned long maxDelay = 1000000L * (long) seconds;
302  struct timeval start_time;
303  vrpn_gettimeofday(&start_time, NULL);
304 
305  int loggedOn = 0;
306  int numRead;
307 
308  if (serial_fd < 0) {
309  return 0;
310  }
311 
312  // ensure that the packet start byte is valid
313  if ( bDataPacketStart != 0x55 && bDataPacketStart != 0xAA ) {
314  bDataPacketStart = 0x55;
315  }
316 
318 
319 // vrpn_write_characters(serial_fd, (const unsigned char *)"E", 1);
320  pause (0.01);
321 
322  while (!loggedOn) {
323  struct timeval current_time;
324  vrpn_gettimeofday(&current_time, NULL);
325  if (vrpn_TimevalDuration(current_time, start_time) > maxDelay ) {
326  // if we've timed out, go back unhappy
327  fprintf(stderr,"vrpn_Tng3::syncDatastream timeout expired: %d secs\n", (int)seconds);
328  return 0; // go back unhappy
329  }
330 
331  // get a byte
332  if (1 != vrpn_read_available_characters(serial_fd, _buffer, 1, &miniDelay)) {
333  continue;
334  }
335  // if not a record start, skip
336  if (_buffer[0] != bDataPacketStart) {
337  continue;
338  }
339  // invert the packet start byte for the next test
340  bDataPacketStart ^= 0xFF;
341 
342  // get an entire report
344  _buffer, DATA_RECORD_LENGTH, &miniDelay);
345 
346  if (numRead < DATA_RECORD_LENGTH) {
347  continue;
348  }
349 
350  // get the start byte for the next packet
351  if (1 != vrpn_read_available_characters(serial_fd, _buffer, 1, &miniDelay)) {
352  continue;
353  }
354 
355  // if not the anticipated record start, things are not yet sync'd
356  if (_buffer[0] != bDataPacketStart) {
357  continue;
358  }
359 
360  // invert the packet start byte in anticipation of normal operation
361  bDataPacketStart ^= 0xFF;
362 
363  // get an entire report
365  _buffer, DATA_RECORD_LENGTH, &miniDelay);
366 
367  if (numRead < DATA_RECORD_LENGTH) {
368  continue;
369  }
370 
371  return 1;
372  }
373  return 0;
374 }
vrpn_Serial_Analog
Definition: vrpn_Analog.h:63
vrpn_Tng3::_timestamp
struct timeval _timestamp
Definition: vrpn_Tng3.h:42
vrpn_Button::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_Analog::status
int status
Definition: vrpn_Analog.h:43
DATA_RECORD_LENGTH
#define DATA_RECORD_LENGTH
Definition: vrpn_Tng3.C:29
vrpn_BaseClass.h
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
MAX_TCHANNELS
#define MAX_TCHANNELS
Definition: vrpn_Tng3.C:21
vrpn_Analog::channel
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
vrpn_Tng3::_numchannels
int _numchannels
Definition: vrpn_Tng3.h:35
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_Tng3.h
vrpn_Analog::timestamp
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_Serial.h
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Button::num_buttons
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_Tng3::get_report
virtual int get_report(void)
Definition: vrpn_Tng3.C:154
vrpn_Button::buttons
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
vrpn_Tng3::mainloop
virtual void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Tng3.C:251
vrpn_TEXT_ERROR
@ vrpn_TEXT_ERROR
Definition: vrpn_BaseClass.h:103
vrpn_flush_input_buffer
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
STATUS_READING
#define STATUS_READING
Definition: vrpn_Tng3.C:35
vrpn_Tng3::report
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
Definition: vrpn_Tng3.C:239
VRPN_MSG_WARNING
#define VRPN_MSG_WARNING(msg)
Definition: vrpn_MessageMacros.h:39
vrpn_Shared.h
MAX_TIME_INTERVAL
#define MAX_TIME_INTERVAL
Definition: vrpn_Tng3.C:36
vrpn_Button::timestamp
struct timeval timestamp
Definition: vrpn_Button.h:48
vrpn_Tng3::vrpn_Tng3
vrpn_Tng3(const char *name, vrpn_Connection *c, const char *port, int baud=19200, const int numbuttons=8, const int numchannels=8)
Definition: vrpn_Tng3.C:58
vrpn_Tng3::_status
int _status
Definition: vrpn_Tng3.h:33
vrpn_Tng3::reset
virtual int reset(void)
Definition: vrpn_Tng3.C:108
vrpn_Serial_Analog::serial_fd
int serial_fd
Definition: vrpn_Analog.h:72
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_Analog::num_channel
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
STATUS_SYNCING
#define STATUS_SYNCING
Definition: vrpn_Tng3.C:34
vrpn_Button::lastbuttons
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
vrpn_read_available_characters
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
vrpn_Tng3::_numbuttons
int _numbuttons
Definition: vrpn_Tng3.h:34
VRPN_BUTTON_OFF
#define VRPN_BUTTON_OFF
Definition: vrpn_Button.h:222
MAX_TBUTTONS
#define MAX_TBUTTONS
Definition: vrpn_Tng3.C:22
vrpn_Tng3::_num_read
unsigned _num_read
Definition: vrpn_Tng3.h:37
vrpn_Analog::last
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39
STATUS_RESETTING
#define STATUS_RESETTING
Definition: vrpn_Tng3.C:33
vrpn_MessageMacros.h
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_BaseClassUnique::send_text_message
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
Definition: vrpn_BaseClass.C:568
vrpn_Tng3::clear_values
virtual void clear_values(void)
Definition: vrpn_Tng3.C:91
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_Tng3::_buffer
unsigned char _buffer[512]
Definition: vrpn_Tng3.h:39
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
VRPN_BUTTON_ON
#define VRPN_BUTTON_ON
Definition: vrpn_Button.h:223