vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Magellan.C
Go to the documentation of this file.
1 // vrpn_Magellan.C
2 // This is a driver for the LogiCad 3D Magellan controller.
3 // This is a 6DOF motion controller with 9 buttons and a 6DOF
4 // puck that can be translated and rotated. It plugs into a serial
5 // line and communicated using RS-232 (this is a raw-mode driver).
6 // You can find out more at www.logicad3d.com; from there you can
7 // download the programming guide, which was used to generate this
8 // driver. This is a driver for the standard Magellan/Space Mouse
9 // (the Turbo Magellan/Space Mouse uses 19200 baud communications
10 // and a compressed data format -- it would only require determining
11 // which mode and implementing a new data translation part to make
12 // the driver compatible with the Turbo version; we don't have one
13 // here to check). The driver was written and tested on a Magellan
14 // version 5.49.
15 
16 // INFO about how the device communicates:
17 // It sends 4-bit nybbles packed into bytes such that there is
18 // even parity: in particular, the following values are sent:
19 // 0 1 2 3 4 5 6 7 8 9 A B C D E F
20 // 30H 41H 42H 33H 44H 35H 36H 47H 48H 39H 3AH 4BH 3CH 4DH 4EH 3FH
21 // '0' 'A' 'B' '3' 'D' '5' '6' 'G' 'H' '9' ':' 'K' '<' 'M' 'N' '?'
22 // Commands are sent starting with a character, then followed
23 // by data, then always terminated by a carriage return '\r'. Useful
24 // commands to send to the device include z\r (zero), vQ\r (query
25 // version), b<\r (beep for half a second),
26 // Julien Brisset found a version of the Magellan that did not
27 // understand the reset command that works on our version, so sent an
28 // alternate reset string that works on his. This is sent if the
29 // 'altreset' parameter is true in the constructor.
30 
31 #include <stdio.h> // for fprintf, stderr
32 #include <string.h> // for strlen, NULL, strcmp
33 
34 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
35 #include "vrpn_Magellan.h"
36 #include "vrpn_Serial.h"
37 #include "vrpn_Shared.h" // for timeval, vrpn_SleepMsecs, etc
38 
39 #undef VERBOSE
40 
41 // Defines the modes in which the box can find itself.
42 #define STATUS_RESETTING (-1) // Resetting the device
43 #define STATUS_SYNCING (0) // Looking for the first character of report
44 #define STATUS_READING (1) // Looking for the rest of the report
45 
46 #define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
47 
48 // This creates a vrpn_Magellan and sets it to reset mode. It opens
49 // the serial device using the code in the vrpn_Serial_Analog constructor.
50 // The box seems to autodetect the baud rate when the "T" command is sent
51 // to it.
53  const char * port, int baud, bool altreset):
54  vrpn_Serial_Analog(name, c, port, baud),
55  vrpn_Button_Filter(name, c),
56  _numbuttons(9),
57  _numchannels(6),
58  _null_radius(8),
59  _altreset(altreset)
60 {
61  // Set the parameters in the parent classes
64 
65  // Set the status of the buttons and analogs to 0 to start
66  clear_values();
67 
68  // Set the mode to reset
70 
71  // Wait before the first time we attempt a reset - seems to be a race condition
72  // with the device needing time between opening of the serial connection and
73  // receiving the reset commands. (SpaceMouse Plus XT Serial, version 6.60)
74  vrpn_SleepMsecs(1000);
75 }
76 
78 {
79  int i;
80 
81  for (i = 0; i < _numbuttons; i++) {
83  }
84  for (i = 0; i < _numchannels; i++) {
86  }
87 }
88 
89 // This routine will reset the Magellan, zeroing the position, setting the compress
90 // mode and making the device beep for half a second. It then verifies that the
91 // commands were received and processed by the device.
92 // Commands Responses Meanings
93 // z\r z\r Set current position and orientation to be zero
94 // m3\r m3\r Set 3D mode, send trans+rot
95 // c30\r c30\r Sets to not compress the return data
96 // nH\r nH\r Sets the NULL radius for the device to 8
97 // bH\r b\r Beep (< means for 32 milliseconds)
98 
100 {
101  struct timeval timeout, now;
102  unsigned char inbuf[45];
103  const char *reset_str = "z\rm3\rc30\rnH\rbH\r"; // Reset string sent to box
104  const char *expect_back = "z\rm3\rc30\rnH\rb\r"; // What we expect back
105  int ret;
106 
107  //-----------------------------------------------------------------------
108  // See if we should be using the alternative reset string.
109  // XXX The "expect_back" string here is almost certainly wrong. Waiting
110  // to hear back what the correct one should be.
111  if (_altreset) {
112  reset_str = "z\rm3\rnH\rp?0\rq00\r";
113  expect_back = "z\rm3\rnH\rp?0\rq00\r";
114  }
115 
116  //-----------------------------------------------------------------------
117  // Set the values back to zero for all buttons, analogs and encoders
118  clear_values();
119 
120  //-----------------------------------------------------------------------
121  // Send the list of commands to the device to cause it to reset and beep.
122  // Read back the response and make sure it matches what we expect.
123  // Give it a reasonable amount of time to finish, then timeout
125  vrpn_write_slowly(serial_fd, (unsigned char *)reset_str, strlen(reset_str), 5);
126  timeout.tv_sec = 1;
127  timeout.tv_usec = 0;
128  ret = vrpn_read_available_characters(serial_fd, inbuf, strlen(expect_back), &timeout);
129  inbuf[strlen(expect_back)] = 0; // Make sure string is NULL-terminated
130 
131  vrpn_gettimeofday(&now, NULL);
132  if (ret < 0) {
133  send_text_message("vrpn_Magellan reset: Error reading from device", now);
134  return -1;
135  }
136  if (ret == 0) {
137  send_text_message("vrpn_Magellan reset: No response from device", now);
138  return -1;
139  }
140  if (ret != (int)strlen(expect_back)) {
141  send_text_message("vrpn_Magellan reset: Got less than expected number of characters", now);
142  //,ret, strlen(expect_back));
143  return -1;
144  }
145 
146  // Make sure the string we got back is what we expected
147  if ( strcmp((char *)inbuf, expect_back) != 0 ) {
148  send_text_message("vrpn_Magellan reset: Bad reset string", now);
149  //(want %s, got %s)\n", expect_back, inbuf);
150  return -1;
151  }
152 
153  // The NULL radius is now set to 8
154  _null_radius = 8;
155 
156  // We're now waiting for a response from the box
158 
159  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
160  return 0;
161 }
162 
163 // This function will read characters until it has a full report, then
164 // put that report into the time, analog, or button fields and call
165 // the report methods on these. The time stored is that of
166 // the first character received as part of the report.
167 // Reports start with different characters, and the length of the report
168 // depends on what the first character of the report is. We switch based
169 // on the first character of the report to see how many more to expect and
170 // to see how to handle the report.
171 // Returns 1 if there is a complete report found, 0 otherwise. This is
172 // so that the calling routine can know to check again at the end of complete
173 // reports to see if there is more than one report buffered up.
174 
176 {
177  int ret; // Return value from function call to be checked
178  int i; // Loop counter
179  int nextchar = 1; // Index of the next character to read
180 
181  //--------------------------------------------------------------------
182  // If we're SYNCing, then the next character we get should be the start
183  // of a report. If we recognize it, go into READing mode and tell how
184  // many characters we expect total. If we don't recognize it, then we
185  // must have misinterpreted a command or something; reset the Magellan
186  // and start over
187  //--------------------------------------------------------------------
188 
189  if (status == STATUS_SYNCING) {
190  // Try to get a character. If none, just return.
192  return 0;
193  }
194 
195  switch (_buffer[0]) {
196  case 'k':
197  _expected_chars = 5; status = STATUS_READING; break;
198  case 'b':
199  _expected_chars = 2; status = STATUS_READING; break;
200  case 'm':
201  _expected_chars = 3; status = STATUS_READING; break;
202  case 'd':
203  _expected_chars = 26; status = STATUS_READING; break;
204  case 'n':
205  _expected_chars = 3; status = STATUS_READING; break;
206  case 'q':
207  _expected_chars = 4; status = STATUS_READING; break;
208  case 'z':
209  _expected_chars = 2; status = STATUS_READING; break;
210  case 'p':
211  _expected_chars = 4; status = STATUS_READING; break;
212  case 'c':
213  _expected_chars = 4; status = STATUS_READING; break;
214 
215  default:
216  fprintf(stderr,"vrpn_Magellan: Unknown command (%c), resetting\n", _buffer[0]);
218  return 0;
219  }
220 
221 
222  // Got the first character of a report -- go into READING mode
223  // and record that we got one character at this time. The next
224  // bit of code will attempt to read the rest of the report.
225  // The time stored here is as close as possible to when the
226  // report was generated.
227  _bufcount = 1;
230 #ifdef VERBOSE
231  printf("... Got the 1st char\n");
232 #endif
233  }
234 
235  //--------------------------------------------------------------------
236  // Read as many bytes of this report as we can, storing them
237  // in the buffer. We keep track of how many have been read so far
238  // and only try to read the rest.
239  //--------------------------------------------------------------------
240 
243  if (ret == -1) {
244  send_text_message("vrpn_Magellan: Error reading", timestamp, vrpn_TEXT_ERROR);
246  return 0;
247  }
248  _bufcount += ret;
249 #ifdef VERBOSE
250  if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
251 #endif
252  if (_bufcount < _expected_chars) { // Not done -- go back for more
253  return 0;
254  }
255 
256  //--------------------------------------------------------------------
257  // We now have enough characters to make a full report. Check to make
258  // sure that its format matches what we expect. If it does, the next
259  // section will parse it. If it does not, we need to go back into
260  // synch mode and ignore this report. A well-formed report has the
261  // last character '\r'
262  //--------------------------------------------------------------------
263 
264  if (_buffer[_expected_chars-1] != '\r') {
266  send_text_message("vrpn_Magellan: No carriage return in record", timestamp, vrpn_TEXT_ERROR);
267  return 0;
268  }
269 
270 #ifdef VERBOSE
271  printf("got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
272 #endif
273 
274  //--------------------------------------------------------------------
275  // Decode the report and store the values in it into the parent classes
276  // (analog or button) if appropriate.
277  //--------------------------------------------------------------------
278 
279  switch ( _buffer[0] ) {
280  case 'k':
281  // This is a button command from the device. It gives us the state
282  // of each of the buttons on the device. Buttons 1-4 are encoded
283  // in the 4 LSBs of the first byte (key 1 in the LSB); buttons 5-8
284  // are in the 4 LSBs of the second byte; the * button (which we'll
285  // call button 0) is in the LSB of the third byte (the other 3 bits
286  // don't seem to be used).
287  buttons[0] = ( (_buffer[3] & 0x01) != 0);
288  buttons[1] = ( (_buffer[1] & 0x01) != 0);
289  buttons[2] = ( (_buffer[1] & 0x02) != 0);
290  buttons[3] = ( (_buffer[1] & 0x04) != 0);
291  buttons[4] = ( (_buffer[1] & 0x08) != 0);
292  buttons[5] = ( (_buffer[2] & 0x01) != 0);
293  buttons[6] = ( (_buffer[2] & 0x02) != 0);
294  buttons[7] = ( (_buffer[2] & 0x04) != 0);
295  buttons[8] = ( (_buffer[2] & 0x08) != 0);
296  break;
297 
298  case 'b':
299  // Beep command received. We don't care.
300  break;
301 
302  case 'm':
303  // Mode set command. We really only care that it is still in
304  // 3D mode (as opposed to Mouse mode); the other fields tell
305  // whether it is in dominant axis mode, and whether translations
306  // and rotations are being sent. We can handle any of these without
307  // incident.
308  if ( (_buffer[1] & 0x08) != 0) {
309  send_text_message("vrpn_Magellan: Was put into mouse mode, resetting", timestamp, vrpn_TEXT_ERROR);
311  return 1;
312  }
313  break;
314 
315  case 'd':
316  // Axis data is being returned (telling what the X,Y,Z and A,B,C axes
317  // are currently set to). This data is put into the range [-1,1] and
318  // put into the analog channels (0=X, 1=Y, 2=Z, 3=A, 4=B, 5=C). It comes
319  // from the device with each axis packed into the lower nybble of 4
320  // consecutive bytes; the translation back to a signed 16-bit integer
321  // is done with (N0 * 4096) + (N1 * 256) + (N2 * 16) + (N3) - 32768
322  // for each value; this is then scaled to [-1,1].
323  nextchar = 1; // Skip the zeroeth character (the command)
324  for (i = 0; i < _numchannels; i++) {
325  long intval;
326  intval = (0x0f & _buffer[nextchar++]) << 12;
327  intval += (0x0f & _buffer[nextchar++]) << 8;
328  intval += (0x0f & _buffer[nextchar++]) << 4;
329  intval += (0x0f & _buffer[nextchar++]);
330  intval -= 32768;
331 
332  // If the absolute value of the integer is <= the NULL radius, it should
333  // be set to zero.
334  if ( (intval <= _null_radius) && (intval >= - _null_radius) ) {
335  intval = 0;
336  }
337 
338  // The largest values that seem to come out of the Magellan I've got
339  // even under the maximum acceleration are absolute value 7200 or so.
340  // We'll divide by 7500 to keep it safe.
341  double realval = intval / 7500.0;
342  channel[i] = realval;
343  }
344  break;
345 
346  case 'n':
347  // NULL radius set. This is the number of ticks around zero that should
348  // count as zero, to allow a "dead zone" for the user near the center.
349  // We store this for the analog parsing code. The low nybble in the data
350  // word holds the new value
351  _null_radius = 0x0f & _buffer[1];
352  break;
353 
354  case 'q':
355  // Sensitivity set. We don't care.
356  break;
357 
358  case 'z':
359  // The device was zeroed. We don't care.
360  break;
361 
362  case 'p':
363  // The min/max periods were set. We don't care.
364  break;
365 
366  case 'c':
367  // Some extended command was sent. I hope we don't care.
368  // XXX Should check to make sure compression is not on.
369  break;
370 
371  default:
372  fprintf(stderr,"vrpn_Magellan: Unknown [internal] command (%c), resetting\n", _buffer[0]);
374  return 1;
375  }
376 
377  //--------------------------------------------------------------------
378  // Done with the decoding, send the reports and go back to syncing
379  //--------------------------------------------------------------------
380 
381  report_changes();
383  _bufcount = 0;
384 
385  return 1; // We got a full report.
386 }
387 
388 void vrpn_Magellan::report_changes(vrpn_uint32 class_of_service)
389 {
392 
393  vrpn_Analog::report_changes(class_of_service);
395 }
396 
397 void vrpn_Magellan::report(vrpn_uint32 class_of_service)
398 {
401 
402  vrpn_Analog::report(class_of_service);
404 }
405 
406 // This routine is called each time through the server's main loop. It will
407 // take a course of action depending on the current status of the Magellan,
408 // either trying to reset it or trying to get a reading from it.
410 {
411  server_mainloop();
412 
413  switch(status) {
414  case STATUS_RESETTING:
415  reset();
416  break;
417 
418  case STATUS_SYNCING:
419  case STATUS_READING:
420  // Keep getting reports until all full reports are read.
421  while (get_report()) {};
422  break;
423 
424  default:
425  fprintf(stderr,"vrpn_Magellan: Unknown mode (internal error)\n");
426  break;
427  }
428 }
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
vrpn_Magellan(const char *name, vrpn_Connection *c, const char *port, int baud, bool altreset=false)
Definition: vrpn_Magellan.C:52
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 vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
#define STATUS_RESETTING
Definition: vrpn_Magellan.C:42
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
unsigned char _buffer[512]
Definition: vrpn_Magellan.h:32
#define STATUS_SYNCING
Definition: vrpn_Magellan.C:43
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_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
unsigned _expected_chars
Definition: vrpn_Magellan.h:31
struct timeval timestamp
Definition: vrpn_Magellan.h:37
#define STATUS_READING
Definition: vrpn_Magellan.C:44
Generic connection class not specific to the transport mechanism.
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
int vrpn_write_slowly(int comm, const unsigned char *buffer, size_t bytes, int millisec_delay)
Definition: vrpn_Serial.C:670
virtual void clear_values(void)
Definition: vrpn_Magellan.C:77
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
virtual void report_changes(void)
Definition: vrpn_Button.C:382
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
unsigned _bufcount
Definition: vrpn_Magellan.h:33
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
virtual int get_report(void)
Try to read a report from the device. Returns 1 if complete report received, 0 otherwise....
virtual void mainloop()
Called once through each main loop iteration to handle updates.
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.
struct timeval timestamp
Definition: vrpn_Button.h:48
virtual int reset(void)
Definition: vrpn_Magellan.C:99
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
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
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39