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