vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Analog_Radamec_SPI.C
Go to the documentation of this file.
1// vrpn_Radamec_SPI.C
2// This is a driver for the Radamec Serial Position Interface
3// "virtual set" camera tracking rig. It plugs into a serial
4// line and communicates using RS-232 (this is a raw-mode driver).
5// You can find out more at www.radamec.com. They have a manual with
6// a section on the serial interface; this code is based on that and
7// communications with the vendor and experimentation. It was written in
8// August 2000 by Russ Taylor.
9
10// INFO about how the device communicates:
11// Reports coming from the device have a 1-byte header that tells
12// which of 7 possible commands is being sent (0xA0-0xA6). Each then has
13// a 1-byte Camera ID, then other parameters specific to the type, then
14// a CRC checksum.
15// Most of the report types can be sent to the instrument to set
16// various parameters. Report type 4 is mainly a command/query message,
17// which the device echoes.
18
19
20#include <stdio.h> // for sprintf
21#include <string.h> // for NULL, memcpy
22
24#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR, etc
25#include "vrpn_Serial.h"
26#include "vrpn_Shared.h" // for timeval, vrpn_unbuffer, etc
27#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
28
29#undef VERBOSE
30
31// Defines the modes in which the device can find itself.
32#define STATUS_RESETTING (-1) // Resetting the device
33#define STATUS_SYNCING (0) // Looking for the first character of report
34#define STATUS_READING (1) // Looking for the rest of the report
35
36#define MAX_TIME_INTERVAL (2000000) // max time between reports (usec)
37
38// This creates a vrpn_Radamec_SPI and sets it to reset mode. It opens
39// the serial device using the code in the vrpn_Serial_Analog constructor.
40
42 const char * port, int baud):
43 vrpn_Serial_Analog(name, c, port, baud, 8, vrpn_SER_PARITY_ODD),
44 _camera_id(-1), // Queried from the controller during reset
45 _numchannels(4) // This is an estimate; will change when reports come
46{
47 // Set the parameters in the parent classes
49
50 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
51
52 // Set the status of the buttons and analogs to 0 to start
54 _bufcount = 0;
55
56 // Set the mode to reset
58}
59
61{
62 int i;
63
64 for (i = 0; i < _numchannels; i++) {
66 }
67}
68
69
78unsigned char vrpn_Radamec_SPI::compute_crc(const unsigned char *head, int len)
79{
80 int i;
81 unsigned char sum;
82
83 // Sum up the bytes, allowing them to overflow the unsigned char
84 sum = 0;
85 for (i = 0; i < len; i++) {
86 sum = (unsigned char)( sum + head[i] );
87 }
88
89 // Unsigned subtraction from 40H, again allowing the subtraction to overflow
90 return (unsigned char)(0x40 - sum);
91}
92
99int vrpn_Radamec_SPI::send_command(const unsigned char *cmd, int len)
100{
101 int ret;
102 unsigned char *outbuf;
103 try { outbuf = new unsigned char[len + 1]; } // Leave room for the CRC
104 catch (...) { return -1; }
105
106 // Put the command into the output buffer
107 memcpy(outbuf, cmd, len);
108
109 // Add the CRC
110 outbuf[len] = compute_crc(cmd, len);
111
112 // Send the command
113 ret = vrpn_write_characters(serial_fd, outbuf, len+1);
114
115 // Tell if this all worked.
116 if (ret == len+1) {
117 return 0;
118 } else {
119 return -1;
120 }
121}
122
123
128vrpn_uint32 vrpn_Radamec_SPI::convert_24bit_unsigned(const unsigned char *buf)
129{
130 vrpn_uint32 retval;
131 unsigned char bigend_buf[4];
132 const unsigned char *bufptr = bigend_buf;
133
134 // Store the three values into three bytes of a big-endian 32-bit integer
135 bigend_buf[0] = 0;
136 bigend_buf[1] = buf[0];
137 bigend_buf[2] = buf[1];
138 bigend_buf[3] = buf[2];
139
140 // Convert the value to an integer
141 vrpn_unbuffer((const char **)&bufptr, &retval);
142 return retval;
143}
144
145
150vrpn_int32 vrpn_Radamec_SPI::convert_16bit_unsigned(const unsigned char *buf)
151{
152 vrpn_int32 retval;
153 unsigned char bigend_buf[4];
154 const unsigned char *bufptr = bigend_buf;
155
156 // Store the three values into two bytes of a big-endian 32-bit integer
157 bigend_buf[0] = 0;
158 bigend_buf[1] = 0;
159 bigend_buf[2] = buf[0];
160 bigend_buf[3] = buf[1];
161
162 // Convert the value to an integer
163 vrpn_unbuffer((const char **)&bufptr, &retval);
164 return retval;
165}
166
185double vrpn_Radamec_SPI::int_to_pan(vrpn_uint32 val)
186{
187 return (((int)val) - 0x7ffff) / 900.0;
188}
189
190double vrpn_Radamec_SPI::int_to_zoom(vrpn_uint32 val)
191{
192 //XXX Unknown conversion, return the raw value
193 return val;
194}
195
196double vrpn_Radamec_SPI::int_to_focus(vrpn_uint32 val)
197{
198 //XXX Unknown conversion, return the raw value
199 return val;
200}
201
202double vrpn_Radamec_SPI::int_to_height(vrpn_uint32 val)
203{
204 //XXX Unknown conversion, send the integer along unchanged
205 return val;
206}
207
211double vrpn_Radamec_SPI::int_to_X(vrpn_uint32 mm, vrpn_uint32 frac)
212{
213 return 0.001 * (mm + frac/65536.0);
214}
215
219{
220 return 0.01 * val;
221}
222
223
224// This routine will reset the Radamec_SPI, requesting the camera number from the
225// device and then turning on stream mode. XXX The device never seems to return a
226// response to these queries, but always seems to go ahead and spew reports, even
227// when not genlocked. If you find a device that doesn't send reports, this may
228// be a good place to look for the problem.
229// Commands Responses Meanings
230// <A4><FF><02><CRC> <A4><##><02><CRC> Request camera number; returns camera number in ##
231// <A4><##><01><CRC> <A4><##><01><CRC> Start stream mode on the camera we were told we are
232
234{
235 unsigned char command[128];
236/* XXX commented out, since the response doesn't come, but the device still works.
237 struct timeval timeout;
238 unsigned char inbuf[128];
239 int ret;
240 char errmsg[256];
241*/
242 //-----------------------------------------------------------------------
243 // Send the command to request the camera ID and then read the response.
244 // Give it a reasonable amount of time to finish (2 seconds), then timeout
245
247 sprintf((char *)command, "%c%c%c", 0xa4, 0xff, 0x02);
248 send_command((unsigned char *)command, 3);
249/* XXX commented out, since the response doesn't come, but the device still works.
250 timeout.tv_sec = 2;
251 timeout.tv_usec = 0;
252 ret = vrpn_read_available_characters(serial_fd, inbuf, 4, &timeout);
253 inbuf[4] = 0; // Make sure string is NULL-terminated
254 if (ret < 0) {
255 perror("vrpn_Radamec_SPI reset: Error reading camera ID from device\n");
256 return -1;
257 }
258 if (ret == 0) {
259 VRPN_MSG_ERROR("reset: No response to camera ID from device");
260 return -1;
261 }
262 if (ret != 4) {
263 sprintf(errmsg,"reset: Got %d of %d expected characters for camera ID\n",ret, 4);
264 VRPN_MSG_ERROR(errmsg);
265 return -1;
266 }
267
268 // Make sure the string we got back is what we expected and then find out the camera ID
269 if ( (inbuf[0] != 0xa4) || (inbuf[2] != 0x02) || (inbuf[3] != compute_crc(inbuf,3)) ) {
270 VRPN_MSG_ERROR("reset: Bad response to camera # request");
271 return -1;
272 }
273 _camera_id = inbuf[1];
274*/
275 //-----------------------------------------------------------------------
276 // Send the command to put the camera into stream mode and then read back
277 // to make sure we got a response.
278
279 sprintf((char *)command, "%c%c%c", 0xa4, _camera_id, 0x01);
280 send_command(command, 3);
281/* XXX commented out, since the response doesn't come, but the device still works.
282 timeout.tv_sec = 2;
283 timeout.tv_usec = 0;
284 ret = vrpn_read_available_characters(serial_fd, inbuf, 4, &timeout);
285 inbuf[4] = 0; // Make sure string is NULL-terminated
286 if (ret < 0) {
287 VRPN_MSG_ERROR("reset: Error reading from device");
288 return -1;
289 }
290 if (ret == 0) {
291 VRPN_MSG_ERROR("reset: No response from device");
292 return -1;
293 }
294 if (ret != 4) {
295 sprintf(errmsg,"vrpn_Radamec_SPI reset: Got %d of %d expected characters\n",ret, 4);
296 VRPN_MSG_ERROR(errmsg);
297 return -1;
298 }
299
300 // Make sure the string we got back is what we expected
301 if ( (inbuf[0] != 0xa4) || (inbuf[1] != _camera_id) || (inbuf[2] != 0x01) ||
302 (inbuf[3] != compute_crc(inbuf,3)) ) {
303 VRPN_MSG_ERROR("reset: Bad response to start stream mode command");
304 return -1;
305 }
306*/
307 // We're now waiting for a response from the box
309
310 VRPN_MSG_WARNING("reset complete (this is good)");
311
312 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
313 return 0;
314}
315
316// This function will read characters until it has a full report, then
317// put that report into analog fields and call the report methods on these.
318// The time stored is that of the first character received as part of the
319// report.
320// Reports start with different characters, and the length of the report
321// depends on what the first character of the report is. We switch based
322// on the first character of the report to see how many more to expect and
323// to see how to handle the report.
324
326{
327 int ret; // Return value from function call to be checked
328 char errmsg[256];
329
330 //--------------------------------------------------------------------
331 // If we're SYNCing, then the next character we get should be the start
332 // of a report. If we recognize it, go into READing mode and tell how
333 // many characters we expect total. If we don't recognize it, then we
334 // must have misinterpreted a command or something; reset the Magellan
335 // and start over
336 //--------------------------------------------------------------------
337
338 if (status == STATUS_SYNCING) {
339 // Try to get a character. If none, just return.
341 return 0;
342 }
343
344 switch (_buffer[0]) {
345 case 0xa0:
347 case 0xa1:
349 case 0xa2:
351 case 0xa3:
353 case 0xa4:
355 case 0xa5:
357 case 0xa6:
359
360 default:
361 // Not a recognized command, keep looking
362 return 0;
363 }
364
365
366 // Got the first character of a report -- go into READING mode
367 // and record that we got one character at this time. The next
368 // bit of code will attempt to read the rest of the report.
369 // The time stored here is as close as possible to when the
370 // report was generated.
371 _bufcount = 1;
374#ifdef VERBOSE
375 printf("... Got the 1st char\n");
376#endif
377 }
378
379 //--------------------------------------------------------------------
380 // Read as many bytes of this report as we can, storing them
381 // in the buffer. We keep track of how many have been read so far
382 // and only try to read the rest.
383 //--------------------------------------------------------------------
384
387 if (ret == -1) {
388 VRPN_MSG_ERROR("Error reading");
390 return 0;
391 }
392 _bufcount += ret;
393#ifdef VERBOSE
394 if (ret != 0) printf("... got %d characters (%d total)\n",ret, _bufcount);
395#endif
396 if (_bufcount < _expected_chars) { // Not done -- go back for more
397 return 0;
398 }
399
400 //--------------------------------------------------------------------
401 // We now have enough characters to make a full report. Check to make
402 // sure that its format matches what we expect. If it does, the next
403 // section will parse it. If it does not, we need to go back into
404 // synch mode and ignore this report. A well-formed report has the
405 // correct CRC as its last character, and has the camera ID matching
406 // what we expect as its second character.
407 //--------------------------------------------------------------------
408
411 VRPN_MSG_WARNING("Bad CRC in report (ignoring this report)");
412 return 0;
413 }
414 _camera_id = _buffer[1];
415
416#ifdef VERBOSE
417 printf("got a complete report (%d of %d)!\n", _bufcount, _expected_chars);
418#endif
419
420 //--------------------------------------------------------------------
421 // Decode the report and store the values in it into the analog values
422 // if appropriate.
423 //--------------------------------------------------------------------
424
425 switch ( _buffer[0] ) {
426 case 0xa0: // Pan, Tilt, Zoom, Focus
427 _numchannels = 4;
432 break;
433
434 case 0xa1: // Pan, Tilt, Zoom, Focus, Height
435 _numchannels = 5;
441 break;
442
443 case 0xa2: // Pan, Tilt, Zoom, Focus, Height, X, Y, Orientation
444 _numchannels = 8;
450 // Note that fraction is first, and is unsigned, in the buffer
453 // Note that fraction is first, and is unsigned, in the buffer
457 break;
458
459 case 0xa4: // Response to our reset commands -- ignore them
460 break;
461
462 // Note that case 0xa3 should never happen, since we don't send this.
463 // We'll let the "default" case complain about getting it.
464
465 // Note that 0xa5 should not happen, since we don't send it.
466 // We'll let the "default" case complain about getting it.
467
468
469 case 0xa6: // Pan, Tilt, Zoom, Focus, Height, X, Y, Orientation
470 // The same as A2, except that fractional X,Y not included.
471 _numchannels = 8;
477 // Note that fraction is not in the buffer
479 // Note that fraction is not in the buffer
482 break;
483
484 default:
485 sprintf(errmsg,"vrpn_Radamec_SPI: Unhandled command (0x%02x), resetting\n", _buffer[0]);
486 VRPN_MSG_ERROR(errmsg);
488 return 0;
489 }
490
491 //--------------------------------------------------------------------
492 // Done with the decoding, send the reports and go back to syncing
493 //--------------------------------------------------------------------
494
495 report(); // Report, rather than report_changes(), since it is an absolute device
497 _bufcount = 0;
498
499 return 1;
500}
501
502void vrpn_Radamec_SPI::report_changes(vrpn_uint32 class_of_service)
503{
505
506 vrpn_Analog::report_changes(class_of_service);
507}
508
509void vrpn_Radamec_SPI::report(vrpn_uint32 class_of_service)
510{
512
513 vrpn_Analog::report(class_of_service);
514}
515
524{
525 char errmsg[256];
526
528
529 switch(status) {
530 case STATUS_RESETTING:
531 reset();
532 break;
533
534 case STATUS_SYNCING:
535 case STATUS_READING:
536 {
537 // It turns out to be important to get the report before checking
538 // to see if it has been too long since the last report. This is
539 // because there is the possibility that some other device running
540 // in the same server may have taken a long time on its last pass
541 // through mainloop(). Trackers that are resetting do this. When
542 // this happens, you can get an infinite loop -- where one tracker
543 // resets and causes the other to timeout, and then it returns the
544 // favor. By checking for the report here, we reset the timestamp
545 // if there is a report ready (ie, if THIS device is still operating).
546 while (get_report()) {}; // Keep getting reports so long as there are more
547
548 struct timeval current_time;
549 vrpn_gettimeofday(&current_time, NULL);
550 if ( vrpn_TimevalDuration(current_time,timestamp) > MAX_TIME_INTERVAL) {
551 sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",
552 current_time.tv_sec, static_cast<long>(current_time.tv_usec),
553 timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
554 VRPN_MSG_ERROR(errmsg);
556 }
557 }
558 break;
559
560 default:
561 VRPN_MSG_ERROR("Unknown mode (internal error)");
562 break;
563 }
564}
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...
Generic connection class not specific to the transport mechanism.
unsigned char compute_crc(const unsigned char *head, int len)
Compute the CRC for the message or report starting at head with length len.
double int_to_orientation(vrpn_uint32 val)
Convert from the 1/100 degree increments into degrees.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
double int_to_focus(vrpn_uint32 val)
double int_to_pan(vrpn_uint32 val)
----------------— Conversion of encoder indices to values -------------— Pan and tilt axis have an en...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report iff changed
vrpn_int32 convert_16bit_unsigned(const unsigned char *buf)
Convert a 16-bit unsigned value from a buffer into an integer.
unsigned char _buffer[512]
double int_to_zoom(vrpn_uint32 val)
double int_to_height(vrpn_uint32 val)
vrpn_Radamec_SPI(const char *name, vrpn_Connection *c, const char *port, int baud=38400)
int send_command(const unsigned char *cmd, int len)
Compute the CRC for the message, append it, and send message. Returns 0 on success,...
double int_to_X(vrpn_uint32 mm, vrpn_uint32 frac)
Convert from the millimeter and fraction-of-millimeter values returned by the device into meters.
double int_to_tilt(vrpn_uint32 val)
virtual int get_report(void)
virtual void mainloop()
Called once through each main loop iteration to handle updates.
vrpn_uint32 convert_24bit_unsigned(const unsigned char *buf)
Convert a 24-bit value from a buffer into an unsigned integer value.
virtual void clear_values(void)
#define STATUS_SYNCING
#define MAX_TIME_INTERVAL
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_ERROR(msg)
#define VRPN_MSG_WARNING(msg)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
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...
@ vrpn_SER_PARITY_ODD
Definition vrpn_Serial.h:17
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.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99