//---------------------------------
void vrpn_Tracker_GPS::reset()
{
if (serial_fd >= 0)
{
FT_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
if (testfile != NULL) fseek(testfile,0L,SEEK_SET);
} else {
status = vrpn_TRACKER_FAIL;
}
nmeaParser.reset();
}
void vrpn_Tracker_InterSense::mainloop()
{
// Call the generic server mainloop, since we are a server
server_mainloop();
switch (status) {
case vrpn_TRACKER_SYNCING:
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
{
// It turns out to be important to get the report before checking
// to see if it has been too long since the last report. This is
// because there is the possibility that some other device running
// in the same server may have taken a long time on its last pass
// through mainloop(). Trackers that are resetting do this. When
// this happens, you can get an infinite loop -- where one tracker
// resets and causes the other to timeout, and then it returns the
// favor. By checking for the report here, we reset the timestamp
// if there is a report ready (ie, if THIS device is still operating).
get_report();
// Ready for another report
status = vrpn_TRACKER_SYNCING;
}
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
FT_WARNING("Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
status = vrpn_TRACKER_RESETTING;
break;
}
}
void vrpn_Tracker_InterSense::reset()
{
#ifdef VRPN_INCLUDE_INTERSENSE
char errStr[1024];
int i;
m_Handle = ISD_OpenTracker(NULL,m_CommPort,FALSE,FALSE);
if(m_Handle == -1)
{
sprintf(errStr,"InterSense: Failed to open tracker '%s' on COM%d: ISD_OpenTracker returned -1",d_servicename,m_CommPort);
fprintf(stderr,errStr);
vrpn_gettimeofday(×tamp, NULL);
FT_ERROR(errStr);
status = vrpn_TRACKER_FAIL;
}
else
{
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format for each of the possible stations.
for (i = 0; i < ISD_MAX_STATIONS; i++) {
if (set_sensor_output_format(i)) {
return;
}
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line.
if (strlen(add_reset_cmd) > 0) {
printf(" Intersense writing extended reset commands...\n");
if(!ISD_SendScript(m_Handle,add_reset_cmd))
{
sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
vrpn_gettimeofday(×tamp, NULL);
FT_WARNING(errStr);
}
}
// If we are using the IS-900 timestamps, clear the timer on the device and
// store the time when we cleared it. First, drain any characters in the output
// buffer to ensure we're sending right away. Then, send the reset command and
// store the time that we sent it, plus the estimated time for the characters to
// get across the serial line to the device at the current baud rate.
// Set time units to milliseconds (MT) and reset the time (MZ).
if (do_is900_timestamps) {
char clear_timestamp_cmd[] = "MT\015MZ\015";
if(!ISD_SendScript(m_Handle,clear_timestamp_cmd))
{
sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
vrpn_gettimeofday(×tamp, NULL);
FT_WARNING(errStr);
}
vrpn_gettimeofday(&is900_zerotime, NULL);
}
// Done with reset.
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
FT_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
#else
fprintf(stderr,"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
status = vrpn_TRACKER_FAIL;
#endif
}
int vrpn_Tracker_InterSense::set_sensor_output_format(int station)
{
char errStr[1024];
//Get the info about the station
ISD_GetStationConfig(m_Handle,&m_StationInfo[station],station+1,0);
//ISD_ResetHeading(m_Handle,station+1); //Not sure if this is needed
// [email protected] -
if (m_reset_at_start)
ISD_Boresight(m_Handle, station+1, true); // equivalent to ISD_ResetHeading for itrax, see isense.h
// First, try to set the orientation reporting format to quaternions if possible.
// But, some models of intersense trackers (like the intertrax series) will only report
// in euler angles
// Try to set the tracker to report in quaternion format
// (to avoid gimbal lock)
// [email protected] : Let's try, even if we are not a precision series
// It seems that this is OK with Intertrax2, what would happen to old Intertrax ?
// if (m_TrackerInfo.TrackerType == ISD_PRECISION_SERIES &&
if (
m_StationInfo[station].AngleFormat == ISD_EULER)
{
m_StationInfo[station].AngleFormat = ISD_QUATERNION;
if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
{
sprintf(errStr,"Warning: Your tracker doesn't seem to support the quaternion format - couldn't set station config for Station%d. ",station+1);
vrpn_gettimeofday(×tamp, NULL);
FT_WARNING(errStr);
m_StationInfo[station].AngleFormat = ISD_EULER;
}
}
// do is900 special things
// IS900 states (timestamp, button, analog).
if(m_TrackerInfo.TrackerModel == ISD_IS900) {
if (do_is900_timestamps) {
m_StationInfo[station].TimeStamped = TRUE;
if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
{
sprintf(errStr,"Warning: Your tracker doesn't seem to support the IS900 timestamps - couldn't set station config for Station%d. ",station+1);
vrpn_gettimeofday(×tamp, NULL);
FT_WARNING(errStr);
m_StationInfo[station].TimeStamped = FALSE;
}
}
if (is900_buttons[station] || is900_analogs[station]) {
m_StationInfo[station].GetInputs = TRUE;
if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
{
sprintf(errStr,"Warning: Your tracker doesn't seem to support the IS900 buttons/analogs - couldn't set station config for Station%d. ",station+1);
vrpn_gettimeofday(×tamp, NULL);
FT_WARNING(errStr);
m_StationInfo[station].GetInputs = FALSE;
}
}
}
return 0;
}
void vrpn_Tracker_Liberty::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
char reset[10];
char errmsg[512];
char outstring1[64],outstring3[64];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// after a few tries with this, use the ^Y reset. Later, try to reset
// to the factory defaults. Then toggle the extended mode.
// Then put in a carriage return to try and break it out of
// a query mode if it is in one. These additions are cumulative: by the
// end, we're doing them all.
fprintf(stderr,"[DEBUG] Beginning Reset");
resetLen = 0;
numResets++; // We're trying another reset
if (numResets > 0) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (char) (13); // Return key -> get ready
reset[resetLen++] = 'F';
reset[resetLen++] = '0';
reset[resetLen++] = (char) (13); // Return key -> get ready
// reset[resetLen++] = (char) (13);
// reset[resetLen++] = (char) (13);
}
/* XXX These commands are probably never needed, and can cause real
headaches for people who are keeping state in their trackers (especially
the InterSense trackers). Taking them out in version 05.01; you can put
them back in if your tracker isn't resetting as well.
if (numResets > 3) { // Get a little more aggressive
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
*/
if (numResets > 2) {
reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
reset[resetLen++] = (char) (13); // Return Key
}
reset[resetLen++] = 'P'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", numResets);
FT_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, (unsigned char*)&reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Liberty: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
// You only need to sleep 10 seconds for an actual Liberty.
// For the Intersense trackers, you need to sleep 20. So,
// sleeping 20 is the more general solution...
if (numResets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
FT_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status. S not implemented in Liberty and hence
// ^V (WhoAmI) is used. It retruns 196 bytes
char statusCommand[2];
statusCommand[0]=(char)(22); // ^V
statusCommand[1]=(char)(13); // Return Key
if (vrpn_write_characters(serial_fd, (const unsigned char *) &statusCommand[0], 2) == 2) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[vrpn_LIBERTY_MAX_WHOAMI_LEN+1];
// Attempt to read whoami_len characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, whoami_len);
if (ret != whoami_len) {
fprintf(stderr," Got %d of %d characters for status\n",ret, whoami_len);
}
// It seems like some versions of the tracker report longer
// messages; so we reduced this chech so that it does not check for the
// appropriate length of message or for the last character being a 10,
// so that it works more generally. The removed tests are:
// || (ret!=whoami_len) || (statusmsg[ret-1]!=(char)(10))
if ( (statusmsg[0]!='0') ) {
int i;
if (ret != -1) {
statusmsg[ret] = '\0'; // Null-terminate the string
}
fprintf(stderr, " Liberty: status is (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,"\n)\n");
FT_ERROR("Bad status report from Liberty, retrying reset");
return;
} else {
FT_WARNING("Liberty/Isense gives status (this is good)");
numResets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format for each of the possible stations.
for (i = 0; i < num_stations; i++) {
if (set_sensor_output_format(i)) {
return;
}
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (X command) and orientation (Y command)
// to the values that are recommended as a "jumping off point" in the
// Liberty manual.
if (do_filter) {
if (DEBUG) fprintf(stderr,"[DEBUG]: Enabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
} else {
if (DEBUG) fprintf(stderr,"[DEBUG]: Disabling filtering\n");
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"X0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd,
(const unsigned char *)"Y0,1,0,0\015", 9) == 9) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Liberty write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Liberty writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
sprintf(outstring1, "F1\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring1,
strlen(outstring1)) == (int)strlen(outstring1)) {
fprintf(stderr, " Liberty set to binary mode\n");
}
// Set tracker to continuous mode
sprintf(outstring3, "C\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring3,
strlen(outstring3)) != (int)strlen(outstring3)) {
perror(" Liberty write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
fprintf(stderr, " Liberty set to continuous mode\n");
}
// If we are using the Liberty timestamps, clear the timer on the device and
// store the time when we cleared it. First, drain any characters in the output
// buffer to ensure we're sending right away. Then, send the reset command and
// store the time that we sent it, plus the estimated time for the characters to
// get across the serial line to the device at the current baud rate.
// Set time units to milliseconds (MT) and reset the time (MZ).
char clear_timestamp_cmd[] = "Q0\r";
vrpn_drain_output_buffer(serial_fd);
if (vrpn_write_characters(serial_fd, (const unsigned char *)clear_timestamp_cmd,
strlen(clear_timestamp_cmd)) != (int)strlen(clear_timestamp_cmd)) {
FT_ERROR("Cannot send command to clear timestamp");
status = vrpn_TRACKER_FAIL;
return;
}
// Drain the output buffer again, then record the time as the base time from
// the tracker.
vrpn_drain_output_buffer(serial_fd);
vrpn_gettimeofday(&liberty_zerotime, NULL);
// Done with reset.
vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
FT_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
void vrpn_Tracker_GPS::mainloop()
{
//char temp[256];
fprintf(stderr,"calling server main\n");
// Call the generic server mainloop, since we are a server
server_mainloop();
//-eb adding get report and removing switch statement
//get_report();
fprintf(stderr,"status in mainloop is %d\n\n",status);
switch (status) {
case vrpn_TRACKER_REPORT_READY:
{
printf("tracker report ready\n",status);
gettimeofday(×tamp, NULL); // Set watchdog now
// Send the message on the connection
if (d_connection) {
char msgbuf[1000];
//sprintf(temp, "position id = %d, sender id = %d", position_m_id, d_sender_id);
//MessageBox(NULL, temp,"GPS Testing",0);
// Pack position report
int len = encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr,"Fastrak: cannot write message: tossing\n");
}
// Pack velocity report
// len = encode_vel_to(msgbuf);
// if (d_connection->pack_message(len, timestamp,
// velocity_m_id, d_sender_id, msgbuf,
// vrpn_CONNECTION_LOW_LATENCY)){
// fprintf(stderr,"Fastrak: cannot write message: tossing\n");
// }
} else {
fprintf(stderr,"Tracker Fastrak: No valid connection\n");
}
// Ready for another report
status = vrpn_TRACKER_SYNCING;
}
break;
case vrpn_TRACKER_SYNCING:
case vrpn_TRACKER_AWAITING_STATION:
case vrpn_TRACKER_PARTIAL:
{
// It turns out to be important to get the report before checking
// to see if it has been too long since the last report. This is
// because there is the possibility that some other device running
// in the same server may have taken a long time on its last pass
// through mainloop(). Trackers that are resetting do this. When
// this happens, you can get an infinite loop -- where one tracker
// resets and causes the other to timeout, and then it returns the
// favor. By checking for the report here, we reset the timestamp
// if there is a report ready (ie, if THIS device is still operating).
get_report();
//
// struct timeval current_time;
// gettimeofday(¤t_time, NULL);
// if ( duration(current_time,timestamp) > MAX_TIME_INTERVAL) {
// sprintf(errmsg,"Timeout... current_time=%ld:%ld, timestamp=%ld:%ld",current_time.tv_sec, current_time.tv_usec, timestamp.tv_sec, timestamp.tv_usec);
// FT_ERROR(errmsg);
// MessageBox(NULL,"Timeout","GPS Testing",0);
// status = vrpn_TRACKER_FAIL;
// }
//
}
break;
case vrpn_TRACKER_RESETTING:
reset();
break;
case vrpn_TRACKER_FAIL:
FT_WARNING("Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
//vrpn_close_commport(serial_fd);
//serial_fd = vrpn_open_commport(portname, baudrate);
status = vrpn_TRACKER_RESETTING;
break;
}//switch
}
void vrpn_Tracker_Isotrak::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
unsigned char reset[10];
char errmsg[512];
//--------------------------------------------------------------------
// This section deals with resetting the tracker to its default state.
// Multiple attempts are made to reset, getting more aggressive each
// time. This section completes when the tracker reports a valid status
// message after the reset has completed.
//--------------------------------------------------------------------
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal 'c' command to put it into polled mode.
// After a few tries with this, use a [return] character, and then use the ^Y to reset.
resetLen = 0;
numResets++;
// We're trying another reset
if (numResets > 1) { // Try to get it out of a query loop if its in one
reset[resetLen++] = (unsigned char) (13); // Return key -> get ready
}
if (numResets > 2) {
reset[resetLen++] = (unsigned char) (25); // Ctrl + Y -> reset the tracker
}
reset[resetLen++] = 'c'; // Put it into polled (not continuous) mode
sprintf(errmsg, "Resetting the tracker (attempt %d)", numResets);
FT_WARNING(errmsg);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
} else {
perror("Isotrack: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
//XXX Take out the sleep and make it keep spinning quickly
if (numResets > 2) {
vrpn_SleepMsecs(1000.0*20); // Sleep to let the reset happen, if we're doing ^Y
}
fprintf(stderr,"\n");
// Get rid of the characters left over from before the reset
vrpn_flush_input_buffer(serial_fd);
// Make sure that the tracker has stopped sending characters
vrpn_SleepMsecs(1000.0*2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
sprintf(errmsg,"Got >=%d characters after reset",ret);
FT_WARNING(errmsg);
for (i = 0; i < ret; i++) {
if (isprint(scrap[i])) {
fprintf(stderr,"%c",scrap[i]);
} else {
fprintf(stderr,"[0x%02X]",scrap[i]);
}
}
fprintf(stderr, "\n");
vrpn_flush_input_buffer(serial_fd); // Flush what's left
}
// Asking for tracker status
if (vrpn_write_characters(serial_fd, (const unsigned char *) "S", 1) == 1) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[22];
// Attempt to read 21 characters.
ret = vrpn_read_available_characters(serial_fd, statusmsg, 21);
if ( (ret != 21) ) {
fprintf(stderr,
" Got %d of 21 characters for status\n",ret);
FT_ERROR("Bad status report from Isotrack, retrying reset");
return;
}
else if ( (statusmsg[0]!='2') ) {
int i;
statusmsg[sizeof(statusmsg) - 1] = '\0'; // Null-terminate the string
fprintf(stderr, " Isotrack: bad status (");
for (i = 0; i < ret; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr,")\n");
FT_ERROR("Bad status report from Isotrack, retrying reset");
return;
} else {
FT_WARNING("Isotrack gives correct status (this is good)");
numResets = 0; // Success, use simple reset next time
}
//--------------------------------------------------------------------
// Now that the tracker has given a valid status report, set all of
// the parameters the way we want them. We rely on power-up setting
// based on the receiver select switches to turn on the receivers that
// the user wants.
//--------------------------------------------------------------------
// Set output format. This is done once for the Isotrak, not per channel.
if (set_sensor_output_format(0)) {
return;
}
// Enable filtering if the constructor parameter said to.
// Set filtering for both position (x command) and orientation (v command)
// to the values that are recommended as a "jumping off point" in the
// Isotrack manual.
if (do_filter) {
if (vrpn_write_characters(serial_fd, (const unsigned char *)"x0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write position filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
if (vrpn_write_characters(serial_fd, (const unsigned char *)"v0.2,0.2,0.8,0.8\015", 17) == 17) {
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
} else {
perror(" Isotrack write orientation filter failed");
status = vrpn_TRACKER_FAIL;
return;
}
}
// RESET Alignment reference frame
if (vrpn_write_characters(serial_fd, (const unsigned char *) "R1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
FT_WARNING("Isotrack reset ALIGNMENT reference frame (this is good)");
}
// reset BORESIGHT
if (vrpn_write_characters(serial_fd, (const unsigned char *) "b1\r", 3) != 3) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
FT_WARNING("Isotrack reset BORESIGHT (this is good)");
}
// Set data format to METRIC mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "u", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
FT_WARNING("Isotrack set to metric units (this is good)");
}
// Send the additional reset commands, if any, to the tracker.
// These commands come in lines, with character \015 ending each
// line. If a line start with an asterisk (*), treat it as a pause
// command, with the number of seconds to wait coming right after
// the asterisk. Otherwise, the line is sent directly to the tracker.
// Wait a while for them to take effect, then clear the input
// buffer.
if (strlen(add_reset_cmd) > 0) {
char *next_line;
char add_cmd_copy[sizeof(add_reset_cmd)];
char string_to_send[sizeof(add_reset_cmd)];
int seconds_to_wait;
printf(" Isotrack writing extended reset commands...\n");
// Make a copy of the additional reset string, since it is consumed
strncpy(add_cmd_copy, add_reset_cmd, sizeof(add_cmd_copy));
// Pass through the string, testing each line to see if it is
// a sleep command or a line to send to the tracker. Continue until
// there are no more line delimiters ('\015'). Be sure to write the
// \015 to the end of the string sent to the tracker.
// Note that strok() puts a NULL character in place of the delimiter.
next_line = strtok(add_cmd_copy, "\015");
while (next_line != NULL) {
if (next_line[0] == '*') { // This is a "sleep" line, see how long
seconds_to_wait = atoi(&next_line[1]);
fprintf(stderr," ...sleeping %d seconds\n",seconds_to_wait);
vrpn_SleepMsecs(1000.0*seconds_to_wait);
} else { // This is a command line, send it
sprintf(string_to_send, "%s\015", next_line);
fprintf(stderr, " ...sending command: %s\n", string_to_send);
vrpn_write_characters(serial_fd,
(const unsigned char *)string_to_send,strlen(string_to_send));
}
next_line = strtok(next_line+strlen(next_line)+1, "\015");
}
// Sleep a little while to let this finish, then clear the input buffer
vrpn_SleepMsecs(1000.0*2);
vrpn_flush_input_buffer(serial_fd);
}
// Set data format to BINARY mode
// F = ASCII, f = binary
if (vrpn_write_characters(serial_fd, (const unsigned char *) "f", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
FT_WARNING("Isotrack set to BINARY mode (this is good)");
}
// Set tracker to continuous mode
if (vrpn_write_characters(serial_fd, (const unsigned char *) "C", 1) != 1) {
perror(" Isotrack write failed");
status = vrpn_TRACKER_FAIL;
return;
} else {
FT_WARNING("Isotrack set to continuous mode (this is good)");
}
FT_WARNING("Reset Completed.");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
// Ok, device is ready, we want to calibrate to sensor 1 current position/orientation
while(get_report() != 1);
// Done with reset.
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_Tracker_Isotrak::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
// The first byte of a binary record has the high order bit set
if (status == vrpn_TRACKER_SYNCING) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, buffer, 1) != 1) {
return 0;
}
// The first byte of a record has the high order bit set
if(!(buffer[0] & 0x80)) {
sprintf(errmsg,"While syncing (looking for byte with high order bit set, "
"got '%x')", buffer[0]);
FT_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Got the first byte of a report -- go into TRACKER_PARTIAL mode
// and record that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}
//--------------------------------------------------------------------
// Read as many bytes of this report as we can, storing them
// in the buffer. We keep track of how many have been read so far
// and only try to read the rest. The routine that calls this one
// makes sure we get a full reading often enough (ie, it is responsible
// for doing the watchdog timing to make sure the tracker hasn't simply
// stopped sending characters).
//--------------------------------------------------------------------
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount],
BINARY_RECORD_SIZE - bufcount);
if (ret == -1) {
FT_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < BINARY_RECORD_SIZE) { // Not done -- go back for more
return 0;
}
// We now have enough characters for a full report
// Check it to ensure we do not have a high bit set other
// than on the first byte
for(int i=1; i<BINARY_RECORD_SIZE; i++)
{
if (buffer[i] & 0x80) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Unexpected sync character in record");
FT_WARNING(errmsg);
//FT_WARNING("Not '0' in record, re-syncing");
vrpn_flush_input_buffer(serial_fd);
return 0;
}
}
// Create a buffer for the decoded message
unsigned char decoded[BINARY_RECORD_SIZE];
int d = 0;
int fullgroups = BINARY_RECORD_SIZE / 8;
// The following decodes the Isotrak binary format. It consists of
// 7 byte values plus an extra byte of the high bit for these
// 7 bytes. First, loop over the 7 byte ranges (8 bytes in binary)
int i;
for(i = 0; i<fullgroups; i++)
{
vrpn_uint8 *group = &buffer[i * 8];
vrpn_uint8 high = buffer[i * 8 + 7];
for(int j=0; j<7; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
}
// We'll have X bytes left at the end
int left = BINARY_RECORD_SIZE - fullgroups * 8;
vrpn_uint8 *group = &buffer[fullgroups * 8];
vrpn_uint8 high = buffer[fullgroups * 8 + left - 1];
for(int j=0; j<left-1; j++)
{
decoded[d] = *group++;
if(high & 1)
decoded[d] |= 0x80;
d++;
high >>= 1;
}
// ASCII value of 1 == 49 subtracing 49 gives the sensor number
d_sensor = decoded[1] - 49; // Convert ASCII 1 to sensor 0 and so on.
if ( (d_sensor < 0) || (d_sensor >= num_stations) ) {
status = vrpn_TRACKER_SYNCING;
sprintf(errmsg,"Bad sensor # (%d) in record, re-syncing", d_sensor);
FT_WARNING(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Extract the important information
vrpn_uint8 *item = &decoded[3];
// This is a scale factor from the Isotrak manual
// This will convert the values to meters, the standard vrpn format
double mul = 1.6632 / 32767.;
float div = 1.f / 32767.f; // Fractional amount for angles
pos[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
pos[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * mul; item += 2;
d_quat[3] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[0] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[1] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div; item += 2;
d_quat[2] = ( (vrpn_int8(item[1]) << 8) + item[0]) * div;
//--------------------------------------------------------------------
// If this sensor has button on it, decode the button values
// into the button device and mainloop the button device so that
// it will report any changes.
//--------------------------------------------------------------------
if(stylus_buttons[d_sensor] != NULL)
{
char button = decoded[2];
if(button == '@' || button == '*')
{
stylus_buttons[d_sensor]->set_button(0, button == '@');
}
stylus_buttons[d_sensor]->mainloop();
}
//--------------------------------------------------------------------
// Done with the decoding,
// set the report to ready
//--------------------------------------------------------------------
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
#ifdef VERBOSE2
print_latest_report();
#endif
return 1;
}
