28#define STATUS_MSG_SECS 600
31 unsigned char uchExpandedErrCode ) {
33 fprintf(stderr,
"\n\rError Code is %u (decimal) ", uchErrCode);
39 fprintf(stderr,
"...No Errors Have Occurred");
42 fprintf(stderr,
"...System RAM Test Error");
45 fprintf(stderr,
"...Non-Volatile Storage Write Failure");
48 fprintf(stderr,
"...System EEPROM Configuration Corrupt");
51 fprintf(stderr,
"...Transmitter EEPROM Configuration Corrupt");
54 fprintf(stderr,
"...Receiver EEPROM Configuration Corrupt");
57 fprintf(stderr,
"...Invalid RS232 Command");
60 fprintf(stderr,
"...Not an FBB Master");
63 fprintf(stderr,
"...No 6DFOBs are Active");
66 fprintf(stderr,
"...6DFOB has not been Initialized");
69 fprintf(stderr,
"...FBB Receive Error - Intra Bird Bus");
72 fprintf(stderr,
"...RS232 Overrun and/or Framing Error");
75 fprintf(stderr,
"...FBB Receive Error - FBB Host Bus");
79 "...No FBB Command Response from Device at address %d (decimal)",
80 uchExpandedErrCode & 0x0f);
83 fprintf(stderr,
"...Invalid FBB Host Command");
86 fprintf(stderr,
"...FBB Run Time Error");
89 fprintf(stderr,
"...Invalid CPU Speed");
92 fprintf(stderr,
"...Slave No Data Error");
95 fprintf(stderr,
"...Illegal Baud Rate");
98 fprintf(stderr,
"...Slave Acknowledge Error");
101 fprintf(stderr,
"...CPU Overflow Error - call factory");
104 fprintf(stderr,
"...Array Bounds Error - call factory");
107 fprintf(stderr,
"...Unused Opcode Error - call factory");
110 fprintf(stderr,
"...Escape Opcode Error - call factory");
113 fprintf(stderr,
"...Reserved Int 9 - call factory");
116 fprintf(stderr,
"...Reserved Int 10 - call factory");
119 fprintf(stderr,
"...Reserved Int 11 - call factory");
122 fprintf(stderr,
"...Numeric CPU Error - call factory");
125 fprintf(stderr,
"...CRT Synchronization Error");
128 fprintf(stderr,
"...Transmitter Not Active Error");
131 fprintf(stderr,
"...ERC Extended Range Transmitter Not Attached Error");
134 fprintf(stderr,
"...CPU Time Overflow Error");
137 fprintf(stderr,
"...Receiver Saturated Error");
140 fprintf(stderr,
"...Slave Configuration Error");
143 fprintf(stderr,
"...ERC Watchdog Error");
146 fprintf(stderr,
"...ERC Overtemp Error");
149 fprintf(stderr,
"...UNKNOWN ERROR... check user manual");
164 unsigned char rguch[2];
171 perror(
"\nvrpn_Tracker_Flock: failed writing cmds to tracker");
190 perror(
"\nvrpn_Tracker_Flock: failed writing cmds to tracker");
204 "\nvrpn_Tracker_Flock: received only %d of 2 chars for err code",
217 int cSensors,
const char *port,
long baud,
218 int fStreamMode,
int useERT,
bool invertQuaternion,
int active_hemisphere)
220 , activeHemisphere(active_hemisphere)
222 , fStream(fStreamMode)
225 , d_invertQuaternion(invertQuaternion)
228 , fFirstStatusReport(1)
234 fprintf(stderr,
"\nvrpn_Tracker_Flock: starting up (FOBHack)...");
241 unsigned char reset[5];
242 unsigned char response[5];
245 reset[resetLen++]=
'O';
247 reset[resetLen++]=
'O';
251 perror(
"\nvrpn_Tracker_Flock: failed writing set mode cmds to tracker");
265 "\nvrpn_Tracker_Flock: received only %d of 4 chars as freq",
271 fprintf(stderr,
"\nvrpn_Tracker_Flock: crystal freq is %d Mhz",
272 (
unsigned int)response[0]);
273 unsigned int iCount = response[2] + (((
unsigned int)response[3]) << 8);
274 return (1000.0/((4*(iCount*(8.0/response[0])/1000.0)) + 0.3));
284 unsigned char rgch [2];
288 fprintf(stderr,
"\nvrpn_Tracker_Flock: shutting down ...");
294 perror(
"\nvrpn_Tracker_Flock: failed writing sleep cmd to tracker");
300 fprintf(stderr,
" done.\n");
335 reset[resetLen++]=
'B';
338 fprintf(stderr,
" vrpn_Flock: Sending POLL mode command...\n");
340 perror(
"\nvrpn_Tracker_Flock: failed writing poll cmd to tracker");
359 reset[resetLen++]=
'P';
362 reset[resetLen++]=50;
371 fprintf(stderr,
" vrpn_Flock: Sending RESET command...\n");
373 perror(
"\nvrpn_Tracker_Flock: failed writing auto-config to tracker");
387 reset[resetLen++] =
'P';
388 reset[resetLen++] = 35;
389 reset[resetLen++] = 1;
394 reset[resetLen++] =
']';
400 unsigned char hem, sign;
403 case HEMI_PLUSX : hem = 0x00; sign = 0x00;
break;
405 case HEMI_PLUSY : hem = 0x06; sign = 0x00;
break;
407 case HEMI_PLUSZ : hem = 0x0c; sign = 0x00;
break;
413 reset[resetLen++] =
'L';
414 reset[resetLen++] = hem;
415 reset[resetLen++] = sign;
419 fprintf(stderr,
" vrpn_Flock: Setting parameters...\n");
421 perror(
"\nvrpn_Tracker_Flock: failed writing set mode cmds to tracker");
436 reset[resetLen++]=
'O';
439 reset[resetLen++]=36;
443 perror(
"\nvrpn_Tracker_Flock: failed writing get sys config to tracker");
454 fprintf(stderr,
" vrpn_Flock: Checking for response...\n");
455 unsigned char response[14];
459 "\nvrpn_Tracker_Flock: received only %d of 14 chars as status",
468 fprintf(stderr,
"\nvrpn_Tracker_Flock: unit %d", i);
469 if (response[i] & 0x20) {
470 fprintf(stderr,
" (a receiver)");
472 fprintf(stderr,
" (a transmitter)");
481 if (response[i] & 0x80) {
482 fprintf(stderr,
" is accessible");
484 fprintf(stderr,
" is not accessible");
487 if (response[i] & 0x40) {
488 fprintf(stderr,
" and is running");
490 fprintf(stderr,
" and is not running");
495 fprintf(stderr,
"\n");
498 perror(
"\nvrpn_Tracker_Flock: problems resetting tracker.");
505 fprintf(stderr,
"\nvrpn_Tracker_Flock: sensor measurement rate is %lf hz.",
514 reset[resetLen++] =
'@';
516 perror(
"\nvrpn_Tracker_Flock: failed writing set mode cmds to tracker");
525 fprintf(stderr,
"\nvrpn_Tracker_Flock: done with reset ... running.\n");
535 unsigned RECORD_SIZE = 15;
561 if ( (
buffer[0] & 0x80) == 0) {
562 fprintf(stderr,
"\nvrpn_Tracker_Flock: Syncing (high bit not set)");
588 fprintf(stderr,
"\nvrpn_Tracker_Flock: Error reading");
598 if ( (
buffer[0] & 0x80) == 0) {
599 fprintf(stderr,
"\nvrpn_Tracker_Flock: lost sync, resyncing.");
609 short *rgs= (
short *)
buffer;
610 short cs = (short)(RECORD_SIZE/2);
620 unsigned char uchLsb;
621 unsigned char uchMsb;
623 for (
int irgs=0;irgs<cs;irgs++) {
626 uchLsb = (
unsigned char)(
buffer[irgs*2] & 0x7F);
628 uchMsb =
buffer[irgs*2+1];
629 rgs[irgs] = (short)( ((
unsigned short) uchLsb) + (((
unsigned short) uchMsb) << 8) );
636 double int_to_inches;
638 int_to_inches = 144.0/32768.0;
640 int_to_inches = 36.0/32768.0;
646 pos[i] = (double)(rgs[i] * int_to_inches * 0.0254);
651#define WTF (float)(1.0/32768.0)
690fprintf(stderr,"."); \
691if (vrpn_write_characters(serial_fd, (const unsigned char *) &chPoint, 1 )!=1) {\
692 perror("\nvrpn_Tracker_Flock: failed writing set mode cmds to tracker");\
693 status = vrpn_TRACKER_FAIL;\
696vrpn_gettimeofday(×tamp, NULL);\
699static const char* vrpn_ctime_r(time_t* cur_time,
char* buffer,
size_t bufsize)
702 if (0 != ctime_s(buffer, bufsize, cur_time)) {
707 return ctime_r(cur_time, buffer);
730 struct timeval tvNow;
741 fprintf(stderr,
"\nFlock: status will be printed every %d seconds.",
753 time_t tNow = time(NULL);
755 memset(pch, 0,
sizeof(pch));
756 vrpn_ctime_r(&tNow, pch,
sizeof(pch));
757 fprintf(stderr,
"\nFlock: reports being sent at %6.2lf hz "
758 "(%d sensors, so ~%6.2lf hz per sensor) ( %s )",
Generic connection class not specific to the transport mechanism.
virtual ~vrpn_Tracker_Flock()
double getMeasurementRate()
virtual void reset()
Reset the tracker.
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_Tracker_Flock(char *name, vrpn_Connection *c, int cSensors=1, const char *port="/dev/ttyd3", long baud=38400, int fStreamMode=1, int useERT=1, bool invertQuaternion=false, int active_hemisphere=HEMI_PLUSZ)
void printError(unsigned char uchErrCode, unsigned char uchExpandedErrCode)
virtual void send_report(void)
struct timeval tvLastStatusReport
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
virtual void send_report(void)
void print_latest_report(void)
#define VRPN_FLOCK_MAX_SENSORS
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_set_rts(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
int vrpn_clear_rts(int comm)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
double vrpn_TimevalMsecs(const timeval &tv)
timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL