vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_3DMouse.C
Go to the documentation of this file.
1 #include <ctype.h> // for isprint
2 #include <math.h> // for cos, sin, M_PI
3 #include <stdio.h> // for fprintf, stderr, perror
4 
5 #ifndef _WIN32
6 #else
7 #define M_PI 3.14159265358979323846
8 #endif
9 
10 #include "vrpn_Serial.h" // for vrpn_write_characters, etc
11 #include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval
12 #include "vrpn_Tracker_3DMouse.h"
13 #include "vrpn_Types.h" // for vrpn_float64
14 
16 
17 // max time between reports (usec)
18 #define MAX_TIME_INTERVAL (2000000)
19 #define INCHES_TO_METERS (2.54/100.0)
20 
22  const char *port, long baud, int filtering_count):
23  vrpn_Tracker_Serial(name, c, port, baud),
24  vrpn_Button_Filter(name, c),
25  _filtering_count(filtering_count),
26  _numbuttons(5),
27  _count(0)
28 {
30  clear_values();
31 }
32 
34 {
35  int i;
36  for (i=0; i <_numbuttons; i++)
38 }
39 
41 {
42 }
43 
45 {
46  int ret, i;
47 
48  clear_values();
49 
50  fprintf(stderr, "Resetting the 3DMouse...\n");
51  if (vrpn_write_characters(serial_fd, (unsigned char*)"*R", 2) == 2)
52  {
53  fprintf(stderr,".");
54  vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
55  }
56  else
57  {
58  perror("3DMouse: Failed writing to 3DMouse");
60  return;
61  }
62 
63  fprintf(stderr,"\n");
64 
65  // Get rid of the characters left over from before the reset
67 
68  // Make sure that the tracker has stopped sending characters
69  vrpn_SleepMsecs(1000.0*2);
70 
71  if ( (ret = vrpn_read_available_characters(serial_fd, _buffer, 80)) != 0)
72  {
73  fprintf(stderr, "Got >=%d characters after reset\n", ret);
74  for (i = 0; i < ret; i++)
75  {
76  if (isprint(_buffer[i])) fprintf(stderr,"%c",_buffer[i]);
77  else fprintf(stderr,"[0x%02X]",_buffer[i]);
78  }
79  fprintf(stderr, "\n");
80  vrpn_flush_input_buffer(serial_fd); // Flush what's left
81  }
82 
83  // Asking for tracker status
84  if (vrpn_write_characters(serial_fd, (const unsigned char *) "*\x05", 2) == 2)
85  vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
86  else
87  {
88  perror(" 3DMouse write failed");
90  return;
91  }
92 
93  // Read Status
94  bool success = true;
95 
97  if (ret != 2) fprintf(stderr, " Got %d of 5 characters for status\n",ret);
98 
99  fprintf(stderr, " Control Unit test : ");
100  if (_buffer[0] & 1) fprintf(stderr, "success\n");
101  else
102  {
103  fprintf(stderr, "fail\n");
104  success = false;
105  }
106 
107  fprintf(stderr, " Processor test : ");
108  if (_buffer[0] & 2) fprintf(stderr, "success\n");
109  else
110  {
111  fprintf(stderr, "fail\n");
112  success = false;
113  }
114 
115  fprintf(stderr, " EPROM checksum test : ");
116  if (_buffer[0] & 4) fprintf(stderr, "success\n");
117  else
118  {
119  fprintf(stderr, "fail\n");
120  success = false;
121  }
122 
123  fprintf(stderr, " RAM checksum test : ");
124  if (_buffer[0] & 8) fprintf(stderr, "success\n");
125  else
126  {
127  fprintf(stderr, "fail\n");
128  success = false;
129  }
130 
131  fprintf(stderr, " Transmitter test : ");
132  if (_buffer[0] & 16) fprintf(stderr, "success\n");
133  else
134  {
135  fprintf(stderr, "fail\n");
136  success = false;
137  }
138 
139  fprintf(stderr, " Receiver test : ");
140  if (_buffer[0] & 32) fprintf(stderr, "success\n");
141  else
142  {
143  fprintf(stderr, "fail\n");
144  success = false;
145  }
146 
147  fprintf(stderr, " Serial Port test : ");
148  if (_buffer[1] & 1) fprintf(stderr, "success\n");
149  else
150  {
151  fprintf(stderr, "fail\n");
152  success = false;
153  }
154 
155  fprintf(stderr, " EEPROM test : ");
156  if (_buffer[0] & 2) fprintf(stderr, "success\n");
157  else
158  {
159  fprintf(stderr, "fail\n");
160  success = false;
161  }
162 
163  if (!success)
164  {
165  fprintf(stderr, "Bad status report from 3DMouse, retrying reset\n");
167  return;
168  }
169  else
170  {
171  fprintf(stderr, "3DMouse gives status (this is good)\n");
172  }
173 
174  // Set filtering count if the constructor parameter said to.
175  if (_filtering_count > 1)
176  {
178  }
179  fprintf(stderr, "Reset Completed (this is good)\n");
180  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
181 }
182 
184 {
185  char sBuf[16];
186 
187  sBuf[0] = 0x2a;
188  sBuf[1] = 0x24;
189  sBuf[2] = 2;
190  sBuf[3] = 7;
191  sBuf[4] = count;
192 
193  if (vrpn_write_characters(serial_fd, (const unsigned char*)sBuf, 5) == 5)
194  {
195  vrpn_SleepMsecs(1000.0*1);
196  }
197  else
198  {
199  perror(" 3DMouse write filtering count failed");
201  return false;
202  }
203 
204  return true;
205 }
206 
208 {
209  int ret; // Return value from function call to be checked
210  timeval waittime;
211  waittime.tv_sec = 2;
212  waittime.tv_usec = 0;
213 
214  if (status == vrpn_TRACKER_SYNCING) {
215  unsigned char tmpc;
216 
217  if (vrpn_write_characters(serial_fd, (const unsigned char*)"*d", 2) !=2)
218  {
219  perror(" 3DMouse write command failed");
221  return 0;
222  }
224  if (ret < 0)
225  {
226  perror(" 3DMouse read failed (disconnected)");
228  return 0;
229  }
230 
231  _count += ret;
232  if (_count < 16) return 0;
233  if (_count > 16)
234  {
235  perror(" 3DMouse read failed (wrong message)");
237  return 0;
238  }
239 
240  _count = 0;
241 
242  tmpc = _buffer[0];
243  if (tmpc & 32)
244  {
245  //printf("port%d: Out of Range\n", i);
246  //ret |= 1 << (3-i);
247  } else
248  {
249  long ax, ay, az; // integer form of absolute translational data
250  short arx, ary, arz; // integer form of absolute rotational data
251  float p, y, r;
252 
253  ax = (_buffer[1] & 0x40) ? 0xFFE00000 : 0;
254  ax |= (long)(_buffer[1] & 0x7f) << 14;
255  ax |= (long)(_buffer[2] & 0x7f) << 7;
256  ax |= (_buffer[3] & 0x7f);
257 
258  ay = (_buffer[4] & 0x40) ? 0xFFE00000 : 0;
259  ay |= (long)(_buffer[4] & 0x7f) << 14;
260  ay |= (long)(_buffer[5] & 0x7f) << 7;
261  ay |= (_buffer[6] & 0x7f);
262 
263  az = (_buffer[7] & 0x40) ? 0xFFE00000 : 0;
264  az |= (long)(_buffer[7] & 0x7f) << 14;
265  az |= (long)(_buffer[8] & 0x7f) << 7;
266  az |= (_buffer[9] & 0x7f);
267 
268  pos[0] = static_cast<float>(ax / 100000.0 * 2.54);
269  pos[2] = static_cast<float>(ay / 100000.0 * 2.54);
270  pos[1] = -static_cast<float>(az / 100000.0f * 2.54);
271 
272  arx = (_buffer[10] & 0x7f) << 7;
273  arx += (_buffer[11] & 0x7f);
274 
275  ary = (_buffer[12] & 0x7f) << 7;
276  ary += (_buffer[13] & 0x7f);
277 
278  arz = (_buffer[14] & 0x7f) << 7;
279  arz += (_buffer[15] & 0x7f);
280 
281  p = static_cast<float>(arx / 40.0); // pitch
282  y = static_cast<float>(ary / 40.0); // yaw
283  r = static_cast<float>(arz / 40.0); // roll
284 
285  p = static_cast<float>(p * M_PI / 180);
286  y = static_cast<float>(y * M_PI / 180);
287  r = static_cast<float>((360-r) * M_PI / 180);
288 
289  float cosp2 = static_cast<float>(cos(p/2));
290  float cosy2 = static_cast<float>(cos(y/2));
291  float cosr2 = static_cast<float>(cos(r/2));
292  float sinp2 = static_cast<float>(sin(p/2));
293  float siny2 = static_cast<float>(sin(y/2));
294  float sinr2 = static_cast<float>(sin(r/2));
295 
296  d_quat[0] = cosr2*sinp2*cosy2 + sinr2*cosp2*siny2;
297  d_quat[1] = sinr2*cosp2*cosy2 + cosr2*sinp2*siny2;
298  d_quat[2] = cosr2*cosp2*siny2 + sinr2*sinp2*cosy2;
299  d_quat[3] = cosr2*cosp2*cosy2 + sinr2*sinp2*siny2;
300 
301  }
302 
303  buttons[0] = tmpc & 16; // Mouse stand button
304  buttons[1] = tmpc & 8; // Suspend button
305  buttons[2] = tmpc & 4; // Left button
306  buttons[3] = tmpc & 2; // Middle button
307  buttons[4] = tmpc & 1; // Right button
308  }
309 
311 
313  bufcount = 0;
314 
315 #ifdef VERBOSE2
317 #endif
318 
319  return 1;
320 }
321 
323 {
324  server_mainloop();
325 
326  switch(status)
327  {
331  if (get_report()) send_report();
332  break;
334  reset();
335  break;
336  case vrpn_TRACKER_FAIL:
337  fprintf(stderr, "3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
338  if (serial_fd >= 0)
339  {
341  serial_fd = -1;
342  }
344  {
345  fprintf(stderr,"vrpn_Tracker_3DMouse::mainloop(): Cannot Open serial port\n");
347  return;
348  }
350  break;
351  default:
352  break;
353  }
354 }
355 
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:345
#define M_PI
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
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
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
virtual void clear_values(void)
unsigned char _buffer[2048]
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Tracker_3DMouse(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int filtering_count=1)
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
Generic connection class not specific to the transport mechanism.
virtual void send_report(void)
Definition: vrpn_Tracker.C:885
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
#define VRPN_API
virtual void reset()
Reset the tracker.
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
virtual void report_changes(void)
Definition: vrpn_Button.C:422
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
char portname[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:151
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
virtual void mainloop()
Called once through each main loop iteration to handle updates.
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
void print_latest_report(void)
Definition: vrpn_Tracker.C:306
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
bool set_filtering_count(int count)
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.