void vrpn_SerialPort::flush_input_buffer() {
requiresOpen();
int ret = vrpn_flush_input_buffer(_comm);
if (ret == -1) {
throw FlushFailure();
}
}
int vrpn_Nikon_Controls::reset(void)
{
unsigned char inbuf[256];
int ret;
char errmsg[256];
char cmd[256];
double response_pos; //< Where the focus says it is.
//-----------------------------------------------------------------------
// Sleep a second and then drain the input buffer to make sure we start
// with a fresh slate.
vrpn_SleepMsecs(1000);
vrpn_flush_input_buffer(serial_fd);
//-----------------------------------------------------------------------
// Send the command to request the focus. Then wait 1 second for a response
sprintf(cmd, "rSPR\r");
if (vrpn_write_characters(serial_fd, (unsigned char *)cmd, strlen(cmd)) != (int)strlen(cmd)) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Cannot send focus request\n");
return -1;
}
vrpn_SleepMsecs(1000);
//-----------------------------------------------------------------------
// Read the response from the camera and then see if it is a good response,
// an error message, or nothing.
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf));
if (ret < 0) {
perror("vrpn_Nikon_Controls::reset(): Error reading position from device");
return -1;
}
if (ret == 0) {
fprintf(stderr, "vrpn_Nikon_Controls::reset(): No characters when reading position from device\n");
return -1;
}
inbuf[ret] = '\0'; //< Null-terminate the input string
ret = parse_focus_position_response((char *)inbuf, response_pos);
if (ret < 0) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Error reading focus: %s\n",
error_code_to_string((int)(response_pos)));
return -1;
}
if (ret != 1) {
fprintf(stderr,"vrpn_Nikon_Controls::reset(): Unexpected response to focus request\n");
return -1;
}
channel[0] = response_pos;
sprintf(errmsg,"Focus reported (this is good)");
VRPN_MSG_WARNING(errmsg);
// We're now waiting for any responses from devices
status = STATUS_SYNCING;
VRPN_MSG_WARNING("reset complete (this is good)");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
// This routine will reset the Spaceball, zeroing the position,
// mode and making the device beep.
int vrpn_Spaceball::reset(void) {
/* Spaceball initialization string. Newer documentation suggests */
/* eliminating several init settings, leaving them at defaults. */
const char *reset_str = "CB\rNT\rFTp\rFRp\rP@r@r\rMSSV\rZ\rBcCcC\r";
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
/* Reset some state variables back to zero */
bufpos = 0;
packtype = 0;
packlen = 0;
escapedchar = 0;
erroroccured = 0;
resetoccured = 0;
spaceball4000 = 0; /* re-determine which type it is */
leftymode4000 = 0; /* re-determine if its in lefty mode */
null_radius = 8; // The NULL radius is now set to 8
// Send commands to the device to cause it to reset and beep.
vrpn_flush_input_buffer(serial_fd);
vrpn_write_slowly(serial_fd, (const unsigned char *)reset_str, strlen(reset_str), 5);
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_Crossbow::recalibrate(vrpn_uint16 num_samples) {
if (num_samples < 100) {
fprintf(stderr, "vrpn_Tracker_Crossbow: Must recalibrate using at least 100 samples\n");
return;
}
else if (num_samples > 25599) {
fprintf(stderr, "vrpn_Tracker_Crossbow: Capping recalibration at 25,500 samples\n");
num_samples = 25500;
}
vrpn_drain_output_buffer(serial_fd);
vrpn_flush_input_buffer(serial_fd);
// Prepare zero command
unsigned char buffer[2];
buffer[0] = 'z';
buffer[1] = (unsigned char) (num_samples / 100);
vrpn_write_characters(serial_fd, buffer, 2);
vrpn_drain_output_buffer(serial_fd);
vrpn_SleepMsecs(50);
// Wait for affirmative response.
// Allow two minutes before timing out.
// Even 25500 samples should make it with a few seconds to spare.
struct timeval timeout;
timeout.tv_sec = 120;
timeout.tv_usec = 0;
if (!vrpn_read_available_characters(serial_fd, buffer, 1, &timeout) || *buffer != 'Z') {
fprintf(stderr, "vrpn_Tracker_Crossbow: Failed to recalibrate device\n");
}
}
int vrpn_inertiamouse::reset(void)
{
clear_values();
vrpn_flush_input_buffer(serial_fd);
status_ = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
int vrpn_GlobalHapticsOrb::reset(void)
{
struct timeval timeout;
unsigned char inbuf[1]; // Response from the Orb
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Clear the input buffer to make sure we're starting with a clean slate.
// Send the "reset" command to the box, then wait for a response and
// make sure it matches what we expect.
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, &reset_char, 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 1, &timeout);
if (ret < 0) {
perror("vrpn_GlobalHapticsOrb::reset(): Error reading from Orb\n");
return -1;
}
if (ret == 0) {
send_text_message("vrpn_GlobalHapticsOrb::reset(): No response from Orb", d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
if (inbuf[0] != 0xfc) {
char message[1024];
sprintf(message, "vrpn_GlobalHapticsOrb::reset(): Bad response from Orb (%02X)", inbuf[0]);
send_text_message(message, d_timestamp, vrpn_TEXT_ERROR);
return -1;
}
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device,
// which is just 1 for the Orb.
d_expected_chars = 1;
vrpn_gettimeofday(&d_timestamp, NULL); // Set watchdog now
send_text_message("vrpn_GlobalHapticsOrb::reset(): Reset complete (this is good)", d_timestamp, vrpn_TEXT_ERROR);
// Set the mode to synchronizing
d_status = STATUS_SYNCING;
return 0;
}
int vrpn_Streaming_Arduino::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all analogs
clear_values();
//-----------------------------------------------------------------------
// Opening the port should cause the board to reset. (We can also do this
// by pulling DTR lines to low and then back high again.)
// @todo Lower and raise DTR lines. Or close and re-open the serial
// port.
// Wait for two seconds to let the device reset itself and then
// clear the input buffer, both the hardware input buffer and
// the local input buffer.
vrpn_SleepMsecs(2100);
vrpn_flush_input_buffer(serial_fd);
m_buffer.clear();
// Send a command telling how many ports we want. Then make sure we
// get a response within a reasonable amount of time.
std::ostringstream msg;
msg << m_numchannels;
msg << "\n";
vrpn_write_characters(serial_fd,
static_cast<const unsigned char *>(
static_cast<const void*>(msg.str().c_str())), msg.str().size());
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 10000;
unsigned char buffer;
int ret = vrpn_read_available_characters(serial_fd, &buffer, 1, &timeout);
if (ret != 1) {
std::cout << "vrpn_Streaming_Arduino: Could not reset" << std::endl;
return -1;
}
m_buffer += buffer;
// We already got the first byte of the record, so we drop directly
// into reading.
status = STATUS_READING;
vrpn_gettimeofday(&m_timestamp, NULL); // Set watchdog now
return 0;
}
int vrpn_BiosciencesTools::reset(void)
{
//-----------------------------------------------------------------------
// Sleep half a second and then drain the input buffer to make sure we start
// with a fresh slate.
vrpn_SleepMsecs(500);
vrpn_flush_input_buffer(serial_fd);
//-----------------------------------------------------------------------
// Set the temperatures for channel 1 and 2 and then set the temperature
// control to be on or off depending on what we've been asked to do.
if (!set_reference_temperature(0, static_cast<float>(o_channel[0]))) {
fprintf(stderr,"vrpn_BiosciencesTools::reset(): Cannot send set ref temp 0, trying again\n");
return -1;
}
if (!set_reference_temperature(1, static_cast<float>(o_channel[1]))) {
fprintf(stderr,"vrpn_BiosciencesTools::reset(): Cannot send set ref temp 1, trying again\n");
return -1;
}
if (!set_control_status(o_channel[0] != 0)) {
fprintf(stderr,"vrpn_BiosciencesTools::reset(): Cannot send set control status, trying again\n");
return -1;
}
//-----------------------------------------------------------------------
// Send the command to request input from the first channel, and set up
// the finite-state machine so we know which thing to request next.
d_next_channel_to_read = 0;
if (!request_temperature(d_next_channel_to_read)) {
fprintf(stderr,"vrpn_BiosciencesTools::reset(): Cannot request temperature, trying again\n");
return -1;
}
// We're now waiting for any responses from devices
status = STATUS_SYNCING;
DO_WARNING("reset complete (this is good)");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_3DMouse::reset()
{
static int numResets = 0; // How many resets have we tried?
int ret, i;
numResets++; // We're trying another reset
clear_values();
fprintf(stderr, "Resetting the 3DMouse (attempt %d)\n", numResets);
if (vrpn_write_characters(serial_fd, (unsigned char*)"*R", 2) == 2)
{
fprintf(stderr,".");
vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
}
else
{
perror("3DMouse: Failed writing to 3DMouse");
status = vrpn_TRACKER_FAIL;
return;
}
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);
if ( (ret = vrpn_read_available_characters(serial_fd, _buffer, 80)) != 0)
{
fprintf(stderr, "Got >=%d characters after reset\n", ret);
for (i = 0; i < ret; i++)
{
if (isprint(_buffer[i])) fprintf(stderr,"%c",_buffer[i]);
else fprintf(stderr,"[0x%02X]",_buffer[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 *) "*\x05", 2) == 2)
vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
else
{
perror(" 3DMouse write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
bool success = true;
ret = vrpn_read_available_characters(serial_fd, _buffer, 2);
if (ret != 2) fprintf(stderr, " Got %d of 5 characters for status\n",ret);
fprintf(stderr, " Control Unit test : ");
if (_buffer[0] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Processor test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EPROM checksum test : ");
if (_buffer[0] & 4) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " RAM checksum test : ");
if (_buffer[0] & 8) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Transmitter test : ");
if (_buffer[0] & 16) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Receiver test : ");
if (_buffer[0] & 32) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " Serial Port test : ");
if (_buffer[1] & 1) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
fprintf(stderr, " EEPROM test : ");
if (_buffer[0] & 2) fprintf(stderr, "success\n");
else
{
fprintf(stderr, "fail\n");
success = false;
}
if (!success)
{
fprintf(stderr, "Bad status report from 3DMouse, retrying reset\n");
status = vrpn_TRACKER_FAIL;
return;
}
else
{
fprintf(stderr, "3DMouse gives status (this is good)\n");
numResets = 0; // Success, use simple reset next time
}
// Set filtering count if the constructor parameter said to.
if (_filtering_count > 1)
{
if (!set_filtering_count(_filtering_count)) return;
}
fprintf(stderr, "Reset Completed (this is good)\n");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_ImmersionBox::reset(void)
{
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// sending an end at this time will force the ibox into the reset mode, if it
// was not already. if the ibox is in the power up mode, nothing will happen because
// it 'should' be waiting to sync up the baudrate
// try to synchronize for 2 seconds
if (syncBaudrate (10.0)) {
printf("vrpn_ImmersionBox found\n");
} else {
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETNAME, (char *) &iname, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETNAME\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPRID, (char *) &id, .1)) {
fprintf(stderr,"problems with ibox command CMD_GETPRID\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETMODL, (char *) &model, .1)){
fprintf(stderr,"problems with ibox command CMD_GETMODL\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETSERN, (char *) &serial, .1)){
fprintf(stderr,"problems with ibox command CMD_GETSERN\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETCOMM, (char *) &comment, .1)){
fprintf(stderr,"problems with ibox command CMD_GETCOMM\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETPERF, (char *) &parmf, .1)){
fprintf(stderr,"problems with ibox command CMD_GETPERF\n");
return -1;
}
if (0 == sendIboxCommand((char) CMD_GETVERS, (char *) &vers, .1)){
fprintf(stderr,"problems with ibox command CMD_GETVERS\n");
return -1;
}
#ifdef VERBOSE
printf("%s\n%s\n%s\n%s\n%s\n%s\n", iname, id, serial, comment, parmf, vers);
#endif
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected.
setupCommand (0, _numchannels, _numencoders);
unsigned char command[26] = "";
command[0] = 0xcf;
command[1] = 0x0;
command[2] = (unsigned char) 10; // milliseconds between reports
command[3] = commandByte;
for (int i = 4; i < 25;i++)
command[i] = 0x0;
//-----------------------------------------------------------------------
int result = vrpn_write_characters(serial_fd, (const unsigned char *) command, 25);
// Ask the box to send a report, ensure echo received
if (result < 25) {
fprintf(stderr,"vrpnImmersionBox::reset: could not write command\n");
return -1;
}
pause (.1);
// look for command echo
result = vrpn_read_available_characters(serial_fd, (unsigned char *) command, 1);
if (result <= 0 || command[0] != 0xcf) {
fprintf(stderr,"vrpnImmersionBox::reset: no command echo\n");
return -1;
}
// flush the input buffer
vrpn_flush_input_buffer(serial_fd);
printf("ImmersionBox reset complete.\n");
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
// sync the baud rate on the ibox.
// seconds determines how long the process is permitted to continue
int vrpn_ImmersionBox::syncBaudrate (double seconds) {
struct timeval miniDelay;
miniDelay.tv_sec = 0;
miniDelay.tv_usec = 50000;
unsigned long maxDelay = 1000000L * (long) seconds;
struct timeval start_time;
vrpn_gettimeofday(&start_time, NULL);
int loggedOn = 0;
unsigned char responseString[8];
const unsigned char * matchString = (unsigned char *) S_INITIALIZE ; // IMMC
int index, numRead;
if (serial_fd < 0)
return 0;
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, (const unsigned char *)"E", 1);
pause (0.01);
while (!loggedOn) {
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if (vrpn_TimevalDuration(current_time, start_time) > maxDelay ) {
// if we've timed out, go back unhappy
fprintf(stderr,"vrpn_ImmersionBox::syncBaudrate timeout expired: %lf secs \n", seconds);
break; // out of while loop
}
// send "IMMC"
if (4 != vrpn_write_characters(serial_fd, matchString, 4)) {
fprintf(stderr,"vrpn_ImmersionBox::syncBaudrate could not write to serial port\n");
break; // out of while loop
}
pause (0.015);
// wait for 4 characters
numRead = vrpn_read_available_characters(serial_fd, responseString, 4, &miniDelay);
if (numRead <= 0)
continue;
// get 4 characters, hopefully "IMMC"
for (index = 0; index < 4; index++) {
// get a character, check for failure
if (responseString[index] != matchString[index])
break;
}
// if we got all four, we're done
if (4 == index)
loggedOn = 1;
}
if (!loggedOn)
return 0;
// now begin the session && ensure that its an ibox we're talking to
matchString = (const unsigned char *) "IBOX";
vrpn_write_characters(serial_fd, (const unsigned char *)"BEGIN", 5);
numRead = vrpn_read_available_characters(serial_fd, responseString, 4, &miniDelay);
if (numRead <= 0)
return 0;
// check 4 characters, hopefully "IBOX"
for (index = 0; index < 4; index++) {
// get a character, check for failure
if (responseString[index] != matchString[index])
return 0;
}
vrpn_flush_input_buffer(serial_fd);
return 1;
}
int vrpn_Tracker_Crossbow::get_report() {
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 500000; // Half a second
switch (status) {
case vrpn_TRACKER_AWAITING_STATION:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
return 0;
case vrpn_TRACKER_SYNCING: {
int rv;
// Quit early if we just got a packet
if (just_read_something) {
just_read_something = 0;
return 0;
}
// Request a packet from the device
unsigned char echo = 'G';
vrpn_write_characters(serial_fd, &echo, 1);
rv = vrpn_read_available_characters(serial_fd, buffer, 1, &timeout);
// Return early if no characters are available
if (!rv)
return 0;
// Return early if we aren't at the start of a packet header
if (*buffer != 0xAA) {
return 0;
}
bufcount = 1;
status = vrpn_TRACKER_PARTIAL;
// Fall through to next state
}
case vrpn_TRACKER_PARTIAL: {
int rv;
if (bufcount == 1) {
rv = vrpn_read_available_characters(serial_fd, buffer + 1, 1, &timeout);
if (!rv || (buffer[1] != 0x55)) {
buffer[0] = 0;
bufcount = 0;
status = vrpn_TRACKER_SYNCING;
return 0;
}
bufcount++;
}
// Try to read the rest of the packet. Return early if we haven't got a full packet yet.
rv = vrpn_read_available_characters(serial_fd, buffer + bufcount, 24 - bufcount, &timeout);
bufcount += rv;
if (bufcount < 24)
return 0;
raw_packet new_data;
unbuffer_packet(new_data, &buffer[0]);
// Ensure the packet is valid
if (validate_packet(new_data)) {
vrpn_flush_input_buffer(serial_fd);
status = vrpn_TRACKER_SYNCING;
return 0;
}
// Prepare the new report
process_packet(new_data);
bufcount = 0;
memset(buffer, 0, 24);
status = vrpn_TRACKER_SYNCING;
just_read_something = 1;
return 1;
}
default:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
return 0;
}
}
int vrpn_Magellan::reset(void)
{
struct timeval timeout, now;
unsigned char inbuf[45];
const char *reset_str = "z\rm3\rc30\rnH\rbH\r"; // Reset string sent to box
const char *expect_back = "z\rm3\rc30\rnH\rb\r"; // What we expect back
int ret;
//-----------------------------------------------------------------------
// See if we should be using the alternative reset string.
// XXX The "expect_back" string here is almost certainly wrong. Waiting
// to hear back what the correct one should be.
if (_altreset) {
reset_str = "z\rm3\rnH\rp?0\rq00\r";
expect_back = "z\rm3\rnH\rp?0\rq00\r";
}
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Send the list of commands to the device to cause it to reset and beep.
// Read back the response and make sure it matches what we expect.
// Give it a reasonable amount of time to finish, then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_slowly(serial_fd, (unsigned char *)reset_str, strlen(reset_str), 5);
timeout.tv_sec = 1;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, strlen(expect_back), &timeout);
inbuf[strlen(expect_back)] = 0; // Make sure string is NULL-terminated
vrpn_gettimeofday(&now, NULL);
if (ret < 0) {
send_text_message("vrpn_Magellan reset: Error reading from device", now);
return -1;
}
if (ret == 0) {
send_text_message("vrpn_Magellan reset: No response from device", now);
return -1;
}
if (ret != (int)strlen(expect_back)) {
send_text_message("vrpn_Magellan reset: Got less than expected number of characters", now);
//,ret, strlen(expect_back));
return -1;
}
// Make sure the string we got back is what we expected
if ( strcmp((char *)inbuf, expect_back) != 0 ) {
send_text_message("vrpn_Magellan reset: Bad reset string", now);
//(want %s, got %s)\n", expect_back, inbuf);
return -1;
}
// The NULL radius is now set to 8
_null_radius = 8;
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
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);
VRPN_MSG_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);
VRPN_MSG_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);
VRPN_MSG_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");
VRPN_MSG_ERROR("Bad status report from Isotrack, retrying reset");
return;
} else {
VRPN_MSG_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 {
VRPN_MSG_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 {
VRPN_MSG_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 {
VRPN_MSG_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 {
VRPN_MSG_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 {
VRPN_MSG_WARNING("Isotrack set to continuous mode (this is good)");
}
VRPN_MSG_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_IDEA::reset(void)
{
struct timeval timeout;
unsigned char inbuf[128];
char cmd[512];
int ret;
//-----------------------------------------------------------------------
// Drain the input buffer to make sure we start with a fresh slate.
// Also wait a bit to let things clear out.
vrpn_SleepMsecs(250);
vrpn_flush_input_buffer(serial_fd);
//-----------------------------------------------------------------------
// Reset the driver, then wait briefly to let it reset.
if (!send_command("R")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not send reset command\n");
return -1;
}
vrpn_SleepMsecs(250);
//-----------------------------------------------------------------------
// Ask for the fault status of the drive. This should cause it to respond
// with an "f" followed by a number and a carriage return. We want the
// number to be zero.
if (!send_command("f")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not request fault status\n");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 30000;
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading fault status from device");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad fault status report (length %d): %s\n", ret, inbuf);
IDEA_ERROR("Bad fault status report");
return -1;
}
inbuf[ret] = '\0';
int fault_status;
if (sscanf((char *)(inbuf), "`f%d\r`f#\r", &fault_status) != 1) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad fault status report: %s\n", inbuf);
IDEA_ERROR("Bad fault status report");
return -1;
}
if (fault_status != 0) {
IDEA_ERROR("Drive reports a fault");
return -1;
}
//-----------------------------------------------------------------------
// Set the outputs to the desired state. If a setting is -1, then we
// don't change the value. If it is 0 or 1 then we set the value to
// what was requested.
int value = 0;
if (d_output_1_setting >= 0) {
value |= 1 << (4 + 0);
value |= (d_output_1_setting != 0) << (0);
}
if (d_output_2_setting >= 0) {
value |= 1 << (4 + 1);
value |= (d_output_2_setting != 0) << (1);
}
if (d_output_3_setting >= 0) {
value |= 1 << (4 + 2);
value |= (d_output_3_setting != 0) << (2);
}
if (d_output_4_setting >= 0) {
value |= 1 << (4 + 3);
value |= (d_output_4_setting != 0) << (3);
}
if (sprintf(cmd, "O%d", value) <= 0) {
IDEA_ERROR("vrpn_IDEA::send_move_request(): Could not configure output command");
status = STATUS_RESETTING;
return -1;
}
if (!send_command(cmd)) {
IDEA_ERROR("vrpn_IDEA::send_move_request(): Could not configure outputs");
status = STATUS_RESETTING;
return -1;
}
//printf("XXX Sending output command: %s\n", cmd);
vrpn_SleepMsecs(100);
//-----------------------------------------------------------------------
// Read the input/output values from the drive (debugging). This is how
// we'd read them to fill in values for the buttons if we want to do that.
if (!send_command(":")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not request I/O status\n");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 30000;
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading I/O status from device");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report (length %d): %s\n", ret, inbuf);
IDEA_ERROR("Bad I/O status report");
return -1;
}
inbuf[ret] = '\0';
int io_status;
if (sscanf((char *)(inbuf), "`:%d\r`:#\r", &io_status) != 1) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad I/O status report: %s\n", inbuf);
IDEA_ERROR("Bad I/O status report");
return -1;
}
//printf("XXX I/O status: %02x\n", io_status);
//-----------------------------------------------------------------------
// Ask for the position of the drive and make sure we can read it.
if (!send_command("l")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not request position\n");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 30000;
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading position from device");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
IDEA_ERROR("Bad position report");
return -1;
}
inbuf[ret] = '\0';
vrpn_float64 position;
if (!convert_report_to_value(inbuf, &position)) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad position report: %s\n", inbuf);
IDEA_ERROR("Bad position report");
return -1;
}
channel[0] = position;
//-----------------------------------------------------------------------
// If we are using the high-limit index, we should abort a move when
// the drive is moving in a positive direction and we get a falling
// edge trigger on the associated input. If the index of the input
// to use is -1, we aren't using this. The motion command will set
// the interrupt handlers appropriately, given the direction of travel.
// Here, we write the programs for the high limit switch and the low
// limit switch that will be called when they are triggered.
if (d_high_limit_index > 0) {
// First, we write the program that will be run when the high limit
// switch goes low. The program will abort the move.
// The program name must be exactly ten characters.
// We put this program at memory location 40 (must be multiple of 4?).
if (!send_command("Phighlimit_,40,1")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not start high limit program\n");
return -1;
}
// The program should cause an abort instruction when run
if (!send_command("A")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not send abort to high limit program\n");
return -1;
}
// The program description is done
if (!send_command("P")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not finish high limit program\n");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 300000;
// Get a response saying that the program has been received; it will
// say what the size of the program is.
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading high-limit program response from device");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad high-limit program response report: %s\n", inbuf);
IDEA_ERROR("Bad position report");
return -1;
}
inbuf[ret] = '\0';
int program_length;
if (sscanf((char *)(inbuf), "`P%d\r`P#\r", &program_length) != 1) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad high program report: %s\n", inbuf);
IDEA_ERROR("Bad high program report");
return -1;
}
}
if (d_low_limit_index > 0) {
// Next, we write the program that will be run when the low limit
// switch goes low. The program will abort the move.
// The program name must be exactly ten characters.
// We put this program at memory location 80 (must be multiple of 4?).
if (!send_command("Plow_limit_,80,1")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not start low limit program\n");
return -1;
}
// The program should cause an abort instruction when run
if (!send_command("A")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not send abort to low limit program\n");
return -1;
}
// The program description is done
if (!send_command("P")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not finish low limit program\n");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 300000;
// Get a response saying that the program has been received; it will
// say what the size of the program is.
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading low-limit program response from device");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad low-limit program response report: %s\n", inbuf);
IDEA_ERROR("Bad low-limit program response report");
return -1;
}
inbuf[ret] = '\0';
int program_length;
if (sscanf((char *)(inbuf), "`P%d\r`P#\r", &program_length) != 1) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad low program report: %s\n", inbuf);
IDEA_ERROR("Bad low program report");
return -1;
}
}
//-----------------------------------------------------------------------
// If we have an initial-move value (to run into the rails), then execute
// a move with acceleration and currents scaled by the fractional_c_a
// value used in the constructor. Wait until the motor stops moving
// after this command before proceeding.
if (d_initial_move != 0) {
// Send a move command, scaled by the fractional current and
// acceleration values.
if (!send_move_request(d_initial_move, d_fractional_c_a)) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not do initial move\n");
IDEA_ERROR("Could not do initial move");
return -1;
}
// Keep asking whether the motor is moving until it says that it
// is not.
bool moving = true;
do {
if (!send_command("o")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not request movement status\n");
IDEA_ERROR("Could not request movement status");
return -1;
}
timeout.tv_sec = 0;
timeout.tv_usec = 30000;
ret = vrpn_read_available_characters(serial_fd, inbuf, sizeof(inbuf), &timeout);
if (ret < 0) {
perror("vrpn_IDEA::reset(): Error reading movement status from device");
IDEA_ERROR("Error reading movement status");
return -1;
}
if ( (ret < 8) || (inbuf[ret-1] != '\r') ) {
inbuf[ret] = '\0';
fprintf(stderr,"vrpn_IDEA::reset(): Bad movement status report (length %d): %s\n", ret, inbuf);
IDEA_ERROR("Bad movement status report");
return -1;
}
inbuf[ret] = '\0';
if ( (inbuf[0] != '`') || (inbuf[1] != 'o') ) {
fprintf(stderr,"vrpn_IDEA::reset(): Bad movement status report: %s\n", inbuf);
IDEA_ERROR("Bad movement status report");
return -1;
}
moving = (inbuf[2] == 'Y');
} while (moving);
}
//-----------------------------------------------------------------------
// Reset the drive count at the present location to zero.
if (!send_command("Z0")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not set position to 0\n");
return -1;
}
//-----------------------------------------------------------------------
// Ask for the position of the drive so that it will start sending.
// them to us. Each time we finish getting a report, we request
// another one.
if (!send_command("l")) {
fprintf(stderr,"vrpn_IDEA::reset(): Could not request position\n");
return -1;
}
// We're now waiting for any responses from devices
IDEA_WARNING("reset complete (this is good)");
vrpn_gettimeofday(&d_timestamp, NULL); // Set watchdog now
status = STATUS_SYNCING;
return 0;
}
vrpn_Tracker_NDI_Polaris::vrpn_Tracker_NDI_Polaris(const char *name,
vrpn_Connection *c,
const char *port,
int numOfRigidBodies,
const char** rigidBodyNDIRomFileNames) : vrpn_Tracker(name,c)
{
/////////////////////////////////////////////////////////
//STEP 1: open com port at NDI's default speed
/////////////////////////////////////////////////////////
serialFd=vrpn_open_commport(port,9600);
if (serialFd==-1){
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: Can't open serial port: %s\n",port);
} else {
printf("connected to NDI Polaris at default 9600 baud on device:%s.\n",port);
//send a reset
#ifdef DEBUG
printf("DEBUG: Reseting com port");
#endif
vrpn_set_rts(serialFd);
vrpn_SleepMsecs(100);
vrpn_clear_rts(serialFd);
printf("done\n");
vrpn_SleepMsecs(100); //if the NDI was previously running at the higher rate, it will need some time to reset
vrpn_flush_input_buffer(serialFd); //get rid of any reset message the NDI might have sent
/////////////////////////////////////////////////////////
//STEP 2: switch to a higher baud rate
/////////////////////////////////////////////////////////
switchToHigherBaudRate(port);
/////////////////////////////////////////////////////////
//STEP 3: INIT tracker
/////////////////////////////////////////////////////////
sendCommand("INIT ");
readResponse();
#ifdef DEBUG
printf("DEBUG:Init response: >%s<\n",latestResponseStr);
#endif
//0 = 20hz(default), 1= 30Hz, 2=60Hz
sendCommand("IRATE 0"); //set the illumination to the default
readResponse();
#ifdef DEBUG
printf("DEBUG: IRATE response: >%s<\n",latestResponseStr);
#endif
/////////////////////////////////////////////////////////
//STEP 4: SETUP EACH TOOL (i.e. rigid body)
/////////////////////////////////////////////////////////
this->numOfRigidBodies=numOfRigidBodies;
//declare an array of filenames, one for each tool
for (int t=0; t<numOfRigidBodies; t++) {
if (setupOneTool(rigidBodyNDIRomFileNames[t])<1) {
fprintf(stderr,"vrpn_Tracker_NDI_Polaris: tool %s didn't init properly!\n",rigidBodyNDIRomFileNames[t]);
}
}
/////////////////////////////////////////////////////////
//STEP 5: GO TO TRACKING MODE
/////////////////////////////////////////////////////////
sendCommand("TSTART 80"); //80 resets the frame counter to zero
readResponse();
#ifdef DEBUG
printf("DEBUG: Tstart response: >%s<\n",latestResponseStr);
#endif
}
}
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]);
VRPN_MSG_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) {
VRPN_MSG_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");
VRPN_MSG_WARNING(errmsg);
//VRPN_MSG_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);
VRPN_MSG_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;
}
int vrpn_Tracker_Liberty::get_report(void)
{
char errmsg[512]; // Error message to send to VRPN
int ret; // Return value from function call to be checked
unsigned char *bufptr; // Points into buffer at the current value to read
//--------------------------------------------------------------------
// Each report starts with the ASCII 'LY' characters. If we're synching,
// read a byte at a time until we find a 'LY' characters.
//--------------------------------------------------------------------
// For the Patriot this is 'PA'.
// For the (high speed) Liberty Latus this is 'LU'.
if (status == vrpn_TRACKER_SYNCING) {
// Try to get the first sync character if don't already have it.
// If none, just return.
if (got_single_sync_char != 1) {
ret = vrpn_read_available_characters(serial_fd, buffer, 1);
if (ret != 1) {
//if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed First Sync Char, ret= %i\n",ret);
return 0;
}
}
// Try to get the second sync character. If none, just return
ret = vrpn_read_available_characters(serial_fd, &buffer[1], 1);
if (ret == 1) {
//Got second sync Char
got_single_sync_char = 0;
}
else if (ret != -1) {
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Missed Second Sync Char\n");
got_single_sync_char = 1;
return 0;
}
// If it is not 'LY' or 'PA' or 'LU' , we don't want it but we
// need to look at the next one, so just return and stay
// in Syncing mode so that we will try again next time through.
// Also, flush the buffer so that it won't take as long to catch up.
if (
((( buffer[0] == 'L') && (buffer[1] == 'Y')) != 1)
&&
((( buffer[0] == 'P') && (buffer[1] == 'A')) != 1)
&&
((( buffer[0] == 'L') && (buffer[1] == 'U')) != 1)
)
{
sprintf(errmsg,"While syncing (looking for 'LY' or 'PA' or 'LU', "
"got '%c%c')", buffer[0], buffer[1]);
VRPN_MSG_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",buffer[0],buffer[1]);
return 0;
}
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Getting Report - Got LY or PA or LU\n");
// Got the first character of a report -- go into AWAITING_STATION mode
// and record that we got one character at this time. The next
// bit of code will attempt to read the station.
// The time stored here is as close as possible to when the
// report was generated. For the InterSense 900 in timestamp
// mode, this value will be overwritten later.
bufcount = 2;
// vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_AWAITING_STATION;
}
//--------------------------------------------------------------------
// The third character of each report is the station number. Once
// we know this, we can compute how long the report should be for the
// given station, based on what values are in its report.
// The station number is converted into a VRPN sensor number, where
// the first Liberty station is '1' and the first VRPN sensor is 0.
//--------------------------------------------------------------------
if (status == vrpn_TRACKER_AWAITING_STATION) {
// Try to get a character. If none, just return.
if (vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1) != 1) {
return 0;
}
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Awaiting Station - Got Station (%i) \n",buffer[2]);
d_sensor = buffer[2] - 1; // 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);
VRPN_MSG_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
// Figure out how long the current report should be based on the
// settings for this sensor.
REPORT_LEN = report_length(d_sensor);
// Got the station report -- to into PARTIAL mode and record
// that we got one character at this time. The next bit of code
// will attempt to read the rest of the report.
bufcount++;
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],
REPORT_LEN-bufcount);
if (ret == -1) {
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Error Reading Report\n");
VRPN_MSG_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < REPORT_LEN) { // Not done -- go back for more
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: Don't have full report (%i of %i)\n",bufcount,REPORT_LEN);
return 0;
}
//--------------------------------------------------------------------
// We now have enough characters to make a full report. Check to make
// sure that its format matches what we expect. If it does, the next
// section will parse it. If it does not, we need to go back into
// synch mode and ignore this report. A well-formed report has the
// first character '0', the next character is the ASCII station
// number, and the third character is either a space or a letter.
//--------------------------------------------------------------------
// fprintf(stderr,"[DEBUG]: Got full report\n");
if (
((buffer[0] != 'L') || (buffer[1] != 'Y'))
&&
((buffer[0] != 'P') || (buffer[1] != 'A'))
&&
((buffer[0] != 'L') || (buffer[1] != 'U'))
) {
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
status = vrpn_TRACKER_SYNCING;
VRPN_MSG_INFO("Not 'LY' or 'PA' or 'LU' in record, re-syncing");
vrpn_flush_input_buffer(serial_fd);
return 0;
}
if (buffer[bufcount-1] != ' ') {
status = vrpn_TRACKER_SYNCING;
VRPN_MSG_INFO("No space character at end of report, re-syncing\n");
vrpn_flush_input_buffer(serial_fd);
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",buffer[bufcount-1], d_sensor);
return 0;
}
//Decode the error status and output a debug message
if (buffer[4] != ' ') {
// An error has been flagged
if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,"[DEBUG]:Error Flag %i\n",buffer[4]);
}
//--------------------------------------------------------------------
// Decode the X,Y,Z of the position and the W,X,Y,Z of the quaternion
// (keeping in mind that we store quaternions as X,Y,Z, W).
//--------------------------------------------------------------------
// The reports coming from the Liberty are in little-endian order,
// which is the opposite of the network-standard byte order that is
// used by VRPN. Here we swap the order to big-endian so that the
// routines below can pull out the values in the correct order.
// This is slightly inefficient on machines that have little-endian
// order to start with, since it means swapping the values twice, but
// that is more than outweighed by the cleanliness gained by keeping
// all architecture-dependent code in the vrpn_Shared.C file.
//--------------------------------------------------------------------
// Point at the first value in the buffer (position of the X value)
bufptr = &buffer[8];
// When copying the positions, convert from inches to meters, since the
// Liberty reports in inches and VRPN reports in meters.
pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * INCHES_TO_METERS;
// Change the order of the quaternion fields to match quatlib order
d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
//--------------------------------------------------------------------
// Decode the time from the Liberty system (unsigned 32bit int), add it to the
// time we zeroed the tracker, and update the report time. Remember
// to convert the MILLIseconds from the report into MICROseconds and
// seconds.
//--------------------------------------------------------------------
struct timeval delta_time; // Time since the clock was reset
// Read the integer value of the time from the record.
vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
// Convert from the float in MILLIseconds to the struct timeval
delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
vrpn_uint32 read_time_milliseconds = read_time - delta_time.tv_sec * 1000; // Subtract out what we just counted
delta_time.tv_usec = (long)(read_time_milliseconds * 1000); // Convert remainder to MICROseconds
// The time that the report was generated
timestamp = vrpn_TimevalSum(liberty_zerotime, delta_time);
vrpn_gettimeofday(&watchdog_timestamp, NULL); // Set watchdog now
//--------------------------------------------------------------------
// 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]) {
// Read the integer value of the bytton status from the record.
vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
stylus_buttons[d_sensor]->set_button(0, button_status);
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;
}
void vrpn_Tracker_Crossbow::reset() {
const char *cmd;
unsigned char recv_buf[8];
struct timeval timeout;
timeout.tv_sec = 1;
timeout.tv_usec = 0;
#if 0 // doesn't help
// First, take the comm port offline for a second
vrpn_close_commport(serial_fd);
vrpn_SleepMsecs(1000);
serial_fd = vrpn_open_commport(portname, baudrate);
#endif
vrpn_flush_output_buffer(serial_fd);
vrpn_flush_input_buffer(serial_fd);
// Try resetting by toggling the RTS line of the serial port
vrpn_set_rts(serial_fd);
vrpn_SleepMsecs(750);
vrpn_clear_rts(serial_fd);
vrpn_SleepMsecs(250);
vrpn_gettimeofday(×tamp, NULL);
vrpn_flush_input_buffer(serial_fd);
cmd = "P";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 1);
vrpn_SleepMsecs(50); // Sleep long enough to stop receiving data
vrpn_flush_input_buffer(serial_fd);
cmd = "RSv";
vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 3);
vrpn_drain_output_buffer(serial_fd);
if (vrpn_read_available_characters(serial_fd, recv_buf, 8, &timeout) != 8) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if ((recv_buf[0] != 'H') || (recv_buf[1] != 255) || (recv_buf[7] != 255)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave unexpected ping response\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (recv_buf[6] != ((recv_buf[2] + recv_buf[3] + recv_buf[4] + recv_buf[5]) & 0xFF)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow gave invalid serial number checksum\n");
status = vrpn_TRACKER_FAIL;
return;
}
const char *bufptr = reinterpret_cast<const char *>(&recv_buf[2]);
vrpn_unbuffer(&bufptr, &device_serial);
if (0) do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
} while (*recv_buf != 255);
int curSize = 4, curLen = 0;
device_version = (char *) realloc(device_version, curSize * sizeof(char));
if (device_version == NULL) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Out of memory\n");
status = vrpn_TRACKER_FAIL;
return;
}
do {
if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
status = vrpn_TRACKER_FAIL;
return;
}
if (*recv_buf != '$')
device_version[curLen++] = *recv_buf;
if (curLen == curSize)
device_version = (char *) realloc(device_version, curSize *= 2);
} while (*recv_buf != '$');
// Now null-terminate the version string, expanding it one last time if necessary
if (curLen == curSize)
device_version = (char *) realloc(device_version, ++curSize);
device_version[curLen] = 0;
//printf("Serial %u\tVersion '%s'\n", device_serial, device_version);
just_read_something = 0;
status = vrpn_TRACKER_SYNCING;
}
/* extern */ error_t
getCmd (handle_t fd, struct command_t* Cmd)
{
struct timeval time_wait;
int ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
unsigned char read_val;
#endif
// some adds to measure time for reading out
#ifdef VRPN_ATMELLIB_TIME_MEASURE
struct timeval start;
struct timeval end;
int sec, usec;
gettimeofday( &start , 0);
#endif
unsigned char PossibilityOne;
unsigned char PossibilityTwo;
unsigned char Reference;
/* check if the given parameters are valid */
if (handle_invalid(fd))
return ENXIO;
if ((*Cmd).addr < 128)
return ATMELLIB_ERROR_NOCOMVAL;
PossibilityOne = (*Cmd).addr;
PossibilityTwo = (*Cmd).addr;
Reference = 0x01;
if (Reference & PossibilityTwo)
/* the LSB of the Address is 1 -> make 0 -> use XOR operator */
PossibilityTwo ^= Reference;
else
/* the LSB of the address is 0 -> make 1 -> use OR operator */
PossibilityTwo |= Reference;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_flush_input_buffer(fd);
vrpn_flush_output_buffer(fd);
#else
tcflush( fd , TCIOFLUSH );
#endif
time_wait.tv_sec = VRPN_ATMELLIB_SELECT_WAIT_SEC;
time_wait.tv_usec = VRPN_ATMELLIB_SELECT_WAIT_USEC;
#ifdef VRPN_ATMEL_SERIAL_VRPN
vrpn_write_characters(fd, (&((*Cmd).addr)) , 1);
#else
/* you have to send the address first */
if ( (ret = select_write_wrapper( fd ,
&(time_wait),
(&((*Cmd).addr)) ,
1 ))
!= 1 ) {
fprintf(stderr, "\n vrpn_atmellib::getCmd: Error while writing down. error=%i\n",
ret);
return ret;
}
#endif
while (time_wait.tv_usec!=0) {
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters for first byte\n");
break;
}
// else
ret = read_val;
#else
if ((ret = select_read_wrapper(fd, &time_wait)) < 0) {
fprintf(stderr, "vrpn_atmellib::getCmd:\
Error select_read_wrapper for first byte: %i\n" , ret);
break;
}
#endif
// found expected first byte
if ((ret==PossibilityOne) || (ret==PossibilityTwo )) {
(*Cmd).addr = ret;
#ifdef VRPN_ATMEL_SERIAL_VRPN
if (( vrpn_read_available_characters(fd, &(read_val), 1, &time_wait)) != 1) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error vrpn_read_available_characters.\n");;
break;
}
//else
ret = read_val;
#else
ret = select_read_wrapper(fd, &time_wait);
#endif
if ((ret < 0) || (ret > 128)) {
fprintf(stderr, "vrpn_atmellib::getCmd: Error reading second byte: %i\n\n" , ret);
break;
}
(*Cmd).value = ret;
#ifdef VRPN_ATMELLIB_TIME_MEASURE
// display time for
gettimeofday( &end , 0);
sec=end.tv_sec-start.tv_sec;
usec=end.tv_usec-start.tv_usec;
printf("Time for reading out: sec=%i , usec=%i\n", sec, usec);
#endif
return ATMELLIB_NOERROR;
}
}
int vrpn_Tng3::syncDatastream (double seconds) {
struct timeval miniDelay;
miniDelay.tv_sec = 0;
miniDelay.tv_usec = 50000;
unsigned long maxDelay = 1000000L * (long) seconds;
struct timeval start_time;
vrpn_gettimeofday(&start_time, NULL);
int loggedOn = 0;
int numRead;
if (serial_fd < 0) {
return 0;
}
// ensure that the packet start byte is valid
if ( bDataPacketStart != 0x55 && bDataPacketStart != 0xAA ) {
bDataPacketStart = 0x55;
}
vrpn_flush_input_buffer(serial_fd);
// vrpn_write_characters(serial_fd, (const unsigned char *)"E", 1);
pause (0.01);
while (!loggedOn) {
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if (duration(current_time, start_time) > maxDelay ) {
// if we've timed out, go back unhappy
fprintf(stderr,"vrpn_Tng3::syncDatastream timeout expired: %d secs\n", (int)seconds);
return 0; // go back unhappy
}
// get a byte
if (1 != vrpn_read_available_characters(serial_fd, _buffer, 1, &miniDelay)) {
continue;
}
// if not a record start, skip
if (_buffer[0] != bDataPacketStart) {
continue;
}
// invert the packet start byte for the next test
bDataPacketStart ^= 0xFF;
// get an entire report
numRead = vrpn_read_available_characters(serial_fd,
_buffer, DATA_RECORD_LENGTH, &miniDelay);
if (numRead < DATA_RECORD_LENGTH) {
continue;
}
// get the start byte for the next packet
if (1 != vrpn_read_available_characters(serial_fd, _buffer, 1, &miniDelay)) {
continue;
}
// if not the anticipated record start, things are not yet sync'd
if (_buffer[0] != bDataPacketStart) {
continue;
}
// invert the packet start byte in anticipation of normal operation
bDataPacketStart ^= 0xFF;
// get an entire report
numRead = vrpn_read_available_characters(serial_fd,
_buffer, DATA_RECORD_LENGTH, &miniDelay);
if (numRead < DATA_RECORD_LENGTH) {
continue;
}
return 1;
}
return 0;
}
//-----------------------------------------------------------------
// This function will read characters until it has a full report, then
// put that report into the time, sensor, pos and quat fields so that it can
// be sent the next time through the loop.
int vrpn_Tracker_GPS::get_report(void)
{
//printf("getting report\n");
char errmsg[512]; // Error message to send to VRPN
//int ret; // Return value from function call to be checked
int ret;
// unsigned char *bufptr; // Points into buffer at the current value to read
int done=0;
//char speed[256];
//char course[256];
// int buflen = 0;
unsigned int numparameter=0;
unsigned int index=0;
// char temp [256];
//--------------------------------------------------------------------
// Each report starts with an ASCII '$' character. If we're synching,
// read a byte at a time until we find a '$' character.
//--------------------------------------------------------------------
if(status == vrpn_TRACKER_SYNCING)
{
// If testfile is live; read data from that
if (testfile != NULL) {
//Read one char to see if test file is active
if(fread(buffer,sizeof(char),1,testfile) != 1)
{
return 0;
}
// Else read from the serial port
} else {
//if(vrpn_read_available_characters(serial_fd, &buffer[0], 1) != 1)
if(vrpn_read_available_characters(serial_fd, buffer, 1) != 1)
{
return 0;
}
}
// If it's not = to $, keep going until the beginning of a packet starts
if( buffer[0] != '$')
{
sprintf(errmsg,"While syncing (looking for '$', got '%c')", buffer[0]);
FT_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
bufcount = 1; // external GPS parser lib expects "$" at start
gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}//if syncing
while (!done)
{
if (testfile != NULL) {
ret = fread(&buffer[bufcount],sizeof(char),1,testfile);
} else {
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1);
}
if (ret == -1) {
FT_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
} else if (ret == 0) {
return 0;
}
bufcount += ret;
if (bufcount >= VRPN_TRACKER_BUF_SIZE*10)
{
status = vrpn_TRACKER_SYNCING;
return 0;
}
if(buffer[bufcount-1] == '\n')
{
buffer[bufcount-1] = '\0';
done = 1;
}
}
if (nmeaParser.parseSentence((char*)buffer) == SENTENCE_VALID)
{
nmeaData = nmeaParser.getData();
if (nmeaData.isValidLat && nmeaData.isValidLon && nmeaData.isValidAltitude)
{
if (useUTM)
{
utmCoord.setLatLonCoord (nmeaData.lat, nmeaData.lon);
if (!utmCoord.isOutsideUTMGrid ())
{
double x, y;
utmCoord.getXYCoord (x,y);
// Christopher 07/25/04: We flip to be x = East <-> West and y = North <-> South
pos[0] = (float)(y);
pos[1] = (float)(x);
pos[2] = (float)(nmeaData.altitude);
}
} else {
pos[0] = (float)(nmeaData.lat);
pos[1] = (float)(nmeaData.lon);
pos[2] = (float)(nmeaData.altitude);
}//use utm d
/*
vel[0] = vel_data[0];
vel[1] = vel_data[1];
vel[2] = vel_data[2];
*/
nmeaParser.reset();
//send report -eb
//-----------------------------
//printf("tracker report ready\n",status);
//gettimeofday(×tamp, NULL); // Set watchdog now
/*
// Send the message on the connection
if (NULL != vrpn_Tracker::d_connection)
{
char msgbuf[1000];
fprintf(stderr, "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,"GPS: cannot write message: tossing\n");
}
else
{
fprintf(stderr,"packed a message\n\n");
}
}
else
{
fprintf(stderr,"Tracker Fastrak: No valid connection\n");
}
//-----------------------------*/
//printf("%s\n", buffer);
//printf("before first sync status is %d\n",status);
status = vrpn_TRACKER_SYNCING;
//printf("after first set sync status is %d\n",status);
return 1;
}//valid lat lon alt
}//valid sentence
// failed valid sentence
//printf("invalid sentence status is %d\n",status);
status = vrpn_TRACKER_SYNCING;
//printf("invalid sentence status syncing\n",status);
return 0;
#ifdef VERBOSE2
// print_latest_report();
#endif
}
// slave resets are ONLY called by the master
void vrpn_Tracker_Flock_Parallel_Slave::reset()
{
// slaves just flush on a reset and then stream (or poll)
// master does all the real resetting
// set vars for error handling
// set them right away so they are set properly in the
// event that we fail during the reset.
cResets++;
cSyncs=0;
fFirstStatusReport=1;
// Get rid of the characters left over from before the reset
// (make sure they are processed)
vrpn_drain_output_buffer(serial_fd);
// put back into polled mode (need to stop stream mode
// before doing an auto-config)
int resetLen=0;
unsigned char reset[3];
reset[resetLen++]='B';
// send the poll mode command (cmd and cmd_size are args)
if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"failed writing poll cmd to tracker", d_sensor);
status = vrpn_TRACKER_FAIL;
return;
}
// make sure the command is sent out
vrpn_drain_output_buffer( serial_fd );
// wait for tracker to respond and flush buffers
vrpn_SleepMsecs(500);
// clear the input buffer (it will contain a single point
// record from the poll command above and garbage from before reset)
vrpn_flush_input_buffer(serial_fd);
// now start it running
resetLen = 0;
// either stream or let poll take place later
if (fStream==1) {
// stream mode
reset[resetLen++] = '@';
if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"failed writing set mode cmds to tracker", d_sensor);
status = vrpn_TRACKER_FAIL;
return;
}
// make sure the commands are sent out
vrpn_drain_output_buffer( serial_fd );
} else {
poll();
}
fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
"done with reset ... running.\n", d_sensor);
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
int vrpn_CerealBox::reset(void)
{
struct timeval timeout;
unsigned char inbuf[45];
const char *Cpy = "Copyright (c), BG Systems";
int major, minor, bug; // Version of the EEPROM
unsigned char reset_str[32]; // Reset string sent to box
int ret;
//-----------------------------------------------------------------------
// Set the values back to zero for all buttons, analogs and encoders
clear_values();
//-----------------------------------------------------------------------
// Check that the box exists and has the correct EEPROM version. The
// "T" command to the box should cause the 44-byte EEPROM string to be
// returned. This string defines the version and type of the box.
// Give it a reasonable amount of time to finish (2 seconds), then timeout
vrpn_flush_input_buffer(serial_fd);
vrpn_write_characters(serial_fd, (const unsigned char *)"T", 1);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 44, &timeout);
inbuf[44] = 0; // Make sure string is NULL-terminated
if (ret < 0) {
perror("vrpn_CerealBox: Error reading from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No response from box\n");
return -1;
}
if (ret != 44) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 44 expected characters\n",ret);
return -1;
}
// Parse the copyright string for the version and other information
// Code here is similar to check_rev() function in the BG example code.
if (strncmp((char *)inbuf, Cpy, strlen(Cpy))) {
fprintf(stderr,"vrpn_CerealBox: Copyright string mismatch: %s\n",inbuf);
return -1;
}
major = inbuf[38] - '0';
minor = inbuf[40] - '0';
bug = inbuf[41] - '0';
if ( (3 != major) || (0 != minor) || (7 != bug) ) {
fprintf(stderr, "vrpn_CerealBox: Bad EEPROM version (want 3.07, got %d.%d%d)\n",
major, minor, bug);
return -1;
}
printf("vrpn_CerealBox: Box of type %c found\n",inbuf[42]);
//-----------------------------------------------------------------------
// Compute the proper string to initialize the device based on how many
// of each type of input/output that is selected. This follows init_cereal()
// in BG example code.
{ int i;
char ana1_4 = 0; // Bits 1-4 do analog channels 1-4
char ana5_8 = 0; // Bits 1-4 do analog channels 5-8
char dig1_3 = 0; // Bits 1-3 enable groups of 8 inputs
char enc1_4 = 0; // Bits 1-4 enable encoders 1-4
char enc5_8 = 0; // Bits 1-4 enable encoders 5-8
// Figure out which analog channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numchannels) {
ana1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numchannels) {
ana5_8 |= (1<<i);
}
}
// Figure out which banks of digital inputs to use and set them
for (i = 0; i < _numbuttons; i++) {
dig1_3 |= (1 << (i/8));
}
// Figure out which encoder channels to use and set them
for (i = 0; i < 4; i++) {
if (i < _numencoders) {
enc1_4 |= (1<<i);
}
}
for (i = 0; i < 4; i++) {
if (i+4 < _numencoders) {
enc5_8 |= (1<<i);
}
}
reset_str[0] = 'c';
reset_str[1] = (unsigned char)(ana1_4 + offset); // Hope we don't need to set baud rate
reset_str[2] = (unsigned char)((ana5_8 | (dig1_3 << 4)) + offset);
reset_str[3] = (unsigned char)(0 + offset);
reset_str[4] = (unsigned char)(0 + offset);
reset_str[5] = (unsigned char)(enc1_4 + offset);
reset_str[6] = (unsigned char)(enc1_4 + offset); // Set encoders 1-4 for incremental
reset_str[7] = (unsigned char)(enc5_8 + offset);
reset_str[8] = (unsigned char)(enc5_8 + offset); // Set encoders 5-8 for incremental
reset_str[9] = '\n';
reset_str[10] = 0;
}
// Send the string and then wait for an acknowledgement from the box.
vrpn_write_characters(serial_fd, reset_str, 10);
timeout.tv_sec = 2;
timeout.tv_usec = 0;
ret = vrpn_read_available_characters(serial_fd, inbuf, 2, &timeout);
if (ret < 0) {
perror("vrpn_CerealBox: Error reading ack from box\n");
return -1;
}
if (ret == 0) {
fprintf(stderr,"vrpn_CerealBox: No ack from box\n");
return -1;
}
if (ret != 2) {
fprintf(stderr,"vrpn_CerealBox: Got %d of 2 expected ack characters\n",ret);
return -1;
}
if (inbuf[0] != 'a') {
fprintf(stderr,"vrpn_CerealBox: Bad ack: wanted 'a', got '%c'\n",inbuf[0]);
return -1;
}
//-----------------------------------------------------------------------
// Ask the box to send a report, and go into SYNCING mode to get it.
vrpn_write_characters(serial_fd, (unsigned char *)"pE", 2);
status = STATUS_SYNCING;
printf("CerealBox reset complete.\n");
//-----------------------------------------------------------------------
// Figure out how many characters to expect in each report from the device
// There is a 'p' to start and '\n' to finish, two bytes for each analog
// value, 4 bytes for each encoder. Buttons are enabled in banks of 8,
// but each bank of 8 is returned in 2 bytes (4 bits each).
_expected_chars = 2 + 2*_numchannels + _numencoders*4 +
((_numbuttons+7) / 8) * 2;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_Liberty::reset()
{
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;
num_resets++; // We're trying another reset
if (num_resets > 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 (num_resets > 3) { // Get a little more aggressive
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
*/
if (num_resets > 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)", num_resets);
VRPN_MSG_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 (num_resets > 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);
VRPN_MSG_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);
}
if (ret != -1) {
statusmsg[ret] = '\0'; // Null-terminate the string
}
// It seems like some versions of the tracker report longer
// messages; so we reduced this check 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;
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");
VRPN_MSG_ERROR("Bad status report from Liberty, retrying reset");
return;
} else {
VRPN_MSG_WARNING("Liberty/Isense gives status (this is good)");
printf("LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
num_resets = 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 (VRPN_LIBERTY_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 (VRPN_LIBERTY_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
vrpn_strcpy(add_cmd_copy, add_reset_cmd);
// 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)) {
VRPN_MSG_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
VRPN_MSG_WARNING("Reset Completed (this is good)");
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
/******************************************************************************
* NAME : vrpn_5dt::reset
* ROLE : This routine will reset the 5DT
* ARGUMENTS :
* RETURN : 0 else -1 in case of error
******************************************************************************/
int
vrpn_5dt::reset (void)
{
struct timeval l_timeout;
unsigned char l_inbuf [45];
int l_ret;
char l_errmsg[256];
if (_wireless) {
// Wireless gloves can't be reset, but we do need to wait for a header byte.
// Then, syncing will wait to see if we get a capability byte as expected
// at the end of a report.
_gotInfo = false;
vrpn_SleepMsecs (100); //Give it time to respond
// Will wait at most 2 seconds
l_timeout.tv_sec = 2;
l_timeout.tv_usec = 0;
l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);
if (l_ret != 1) {
_5DT_ERROR ("vrpn_5dt: Unable to read from the glove\n");
return -1;
}
if (l_inbuf[0] == 0x80) {
_status = STATUS_SYNCING;
_buffer[0] = l_inbuf[0];
_bufcount = 1;
vrpn_gettimeofday (×tamp, NULL); // Set watchdog now
_5DT_INFO ("vrpn_5dt: Got a possible header byte!");
return 0;
}
return 0;
}
vrpn_flush_input_buffer (serial_fd);
send_command ((unsigned char *) "A", 1); // Command to init the glove
vrpn_SleepMsecs (100); //Give it time to respond
l_timeout.tv_sec = 2;
l_timeout.tv_usec = 0;
l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 1, &l_timeout);
if (l_ret != 1) {
_5DT_ERROR ("vrpn_5dt: Unable to read from the glove\n");
return -1;
}
if (l_inbuf[0] != 85) {
_5DT_ERROR ("vrpn_5dt: Cannot get response on init command");
return -1;
} else {
vrpn_flush_input_buffer (serial_fd);
send_command ( (unsigned char *) "G", 1); //Command to Query informations from the glove
vrpn_SleepMsecs (100); //Give it time to respond
l_timeout.tv_sec = 2;
l_timeout.tv_usec = 0;
l_ret = vrpn_read_available_characters (serial_fd, l_inbuf, 32, &l_timeout);
if (l_ret != 32) {
_5DT_ERROR ("vrpn_5dt: Cannot get info. from the glove");
return -1;
}
if ( (l_inbuf[0] != 66) || (l_inbuf[1] != 82)) {
_5DT_ERROR ("vrpn_5dt: Cannot get good header on info command");
return -1;
}
sprintf (l_errmsg, "vrpn_5dt: glove \"%s\"version %d.%d\n", &l_inbuf [16], l_inbuf [2], l_inbuf [3]);
_5DT_INFO (l_errmsg);
if (l_inbuf[4] | 1) {
_5DT_INFO ("A right glove is ready");
} else {
_5DT_INFO ("A left glove is ready");
}
if (l_inbuf[5] | 16) {
_5DT_INFO ("Pitch and Roll are available");
} else {
_5DT_INFO ("Pitch and Roll are not available");
}
}
// If we're in continuous mode, request continuous sends
if (_mode == 2) {
send_command ( (unsigned char *) "D", 1); // Command to query streaming data from the glove
}
// We're now entering the syncing mode which send the read command to the glove
_status = STATUS_SYNCING;
vrpn_gettimeofday (×tamp, NULL); // Set watchdog now
return 0;
}
void vrpn_Tracker_3Space::reset()
{
static int numResets = 0; // How many resets have we tried?
int i,resetLen,ret;
unsigned char reset[10];
// Send the tracker a string that should reset it. The first time we
// try this, just do the normal ^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.
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++] = (char) (13); // Return key -> get ready
}
if (numResets > 7) {
reset[resetLen++] = 'Y'; // Put tracker into tracking (not point) mode
}
if (numResets > 3) { // Get a little more aggressive
if (numResets > 4) { // Even more aggressive
reset[resetLen++] = 't'; // Toggle extended mode (in case it is on)
}
reset[resetLen++] = 'W'; // Reset to factory defaults
reset[resetLen++] = (char) (11); // Ctrl + k --> Burn settings into EPROM
}
reset[resetLen++] = (char) (25); // Ctrl + Y -> reset the tracker
send_text_message("Resetting", timestamp, vrpn_TEXT_ERROR, numResets);
for (i = 0; i < resetLen; i++) {
if (vrpn_write_characters(serial_fd, &reset[i], 1) == 1) {
sleep(2); // Wait 2 seconds each character
} else {
send_text_message("Failed writing to tracker", timestamp, vrpn_TEXT_ERROR, numResets);
perror("3Space: Failed writing to tracker");
status = vrpn_TRACKER_FAIL;
return;
}
}
sleep(10); // Sleep to let the reset happen
// 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
sleep(2);
unsigned char scrap[80];
if ( (ret = vrpn_read_available_characters(serial_fd, scrap, 80)) != 0) {
fprintf(stderr," 3Space warning: got >=%d characters after reset:\n",ret);
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) {
sleep(1); // Sleep for a second to let it respond
} else {
perror(" 3Space write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Read Status
unsigned char statusmsg[56];
if ( (ret = vrpn_read_available_characters(serial_fd, statusmsg, 55)) != 55){
fprintf(stderr, " Got %d of 55 characters for status\n",ret);
}
if ( (statusmsg[0]!='2') || (statusmsg[54]!=(char)(10)) ) {
int i;
statusmsg[55] = '\0'; // Null-terminate the string
fprintf(stderr, " Tracker: status is (");
for (i = 0; i < 55; i++) {
if (isprint(statusmsg[i])) {
fprintf(stderr,"%c",statusmsg[i]);
} else {
fprintf(stderr,"[0x%02X]",statusmsg[i]);
}
}
fprintf(stderr, ")\n Bad status report from tracker, retrying reset\n");
return;
} else {
send_text_message("Got status (tracker back up)!", timestamp, vrpn_TEXT_ERROR, 0);
numResets = 0; // Success, use simple reset next time
}
// Set output format to be position,quaternion
// These are a capitol 'o' followed by comma-separated values that
// indicate data sets according to appendix F of the 3Space manual,
// then followed by character 13 (octal 15).
if (vrpn_write_characters(serial_fd, (const unsigned char *)"O2,11\015", 6) == 6) {
sleep(1); // Sleep for a second to let it respond
} else {
perror(" 3Space write failed");
status = vrpn_TRACKER_FAIL;
return;
}
// Set data format to BINARY mode
vrpn_write_characters(serial_fd, (const unsigned char *)"f", 1);
// Set tracker to continuous mode
if (vrpn_write_characters(serial_fd,(const unsigned char *) "C", 1) != 1)
perror(" 3Space write failed");
else {
fprintf(stderr, " 3Space set to continuous mode\n");
}
fprintf(stderr, " (at the end of 3Space reset routine)\n");
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
}
//-----------------------------------------------------------------
// This function will read characters until it has a full report, then
// put that report into the time, sensor, pos and quat fields so that it can
// be sent the next time through the loop.
int vrpn_Tracker_GPS::get_report(void)
{
//printf("getting report\n");
char errmsg[512]; // Error message to send to VRPN
//int ret; // Return value from function call to be checked
int ret;
// unsigned char *bufptr; // Points into buffer at the current value to read
int done=0;
//char speed[256];
//char course[256];
// int buflen = 0;
int charRead = 0;
// char temp [256];
//--------------------------------------------------------------------
// Each report starts with an ASCII '$' character. If we're synching,
// read a byte at a time until we find a '$' character.
//--------------------------------------------------------------------
//printf("STATUS =%d\n", status);
if(status == vrpn_TRACKER_SYNCING)
{
// If testfile is live; read data from that
if (testfile != NULL) {
//Read one char to see if test file is active
if(fread(buffer,sizeof(char),1,testfile) != 1)
{
return 0;
}
// Else read from the serial port
} else {
charRead = vrpn_read_available_characters(serial_fd, buffer, 1);
if( charRead == -1)
{
printf("fail to read first bit\n");
return 0;
} else if (charRead == 0) {
//case of successful read but no new bits, take a quick nap, wait for new info.
#ifdef _WIN32
Sleep(10);
#else
usleep(10000);
#endif
return 0;
}
}
//if it returns 0 (very likely - happens on interrupt or no new bits) then we have to prevent it from reading the same buffer over and over.
// If it's not = to $, keep going until the beginning of a packet starts
if( buffer[0] != '$')
{
sprintf(errmsg,"While syncing (looking for '$', got '%c')", buffer[0]);
VRPN_MSG_INFO(errmsg);
vrpn_flush_input_buffer(serial_fd);
return 0;
}
//printf("We have a $, setting to partial and moving on.\n");
bufcount = 1; // external GPS parser lib expects "$" at start
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}//if syncing
while (!done)
{
if (testfile != NULL) {
ret = static_cast<int>(fread(&buffer[bufcount],sizeof(char),1,testfile));
} else {
ret = vrpn_read_available_characters(serial_fd, &buffer[bufcount], 1);
}
if (ret == -1) {
VRPN_MSG_ERROR("Error reading report");
status = vrpn_TRACKER_FAIL;
return 0;
} else if (ret == 0) {
return 0;
}
bufcount += ret;
if (bufcount >= VRPN_TRACKER_BUF_SIZE*10)
{
status = vrpn_TRACKER_SYNCING;
return 0;
}
if(buffer[bufcount-1] == '\n')
{
buffer[bufcount-1] = '\0';
done = 1;
}
}
if (nmeaParser.parseSentence((char*)buffer) == SENTENCE_VALID)
{
nmeaData = nmeaParser.getData();
//printf("Raw lastfixQuality: %d anyValid: %d LAT: %f LONG: %f ALT: %f\n", nmeaData.lastFixQuality, nmeaData.hasCoordEverBeenValid, nmeaData.lat, nmeaData.lon, nmeaData.altitude);
//required RMC doesn't do altitude, so this will be needlessly false half of the time.
//if (nmeaData.isValidLat && nmeaData.isValidLon && nmeaData.isValidAltitude)
if (nmeaData.isValidLat && nmeaData.isValidLon)
{
if(nmeaData.isValidAltitude){
if (useUTM)
{
utmCoord.setLatLonCoord (nmeaData.lat, nmeaData.lon);
if (!utmCoord.isOutsideUTMGrid ())
{
double x, y;
utmCoord.getXYCoord (x,y);
// Christopher 07/25/04: We flip to be x = East <-> West and y = North <-> South
//Should this be changed to match rtk precision vrpn_float64?
pos[0] = (float)(y);
pos[1] = (float)(x);
pos[2] = (float)(nmeaData.altitude);
}
} else {
pos[0] = (vrpn_float64)(nmeaData.lat);
pos[1] = (vrpn_float64)(nmeaData.lon);
pos[2] = (vrpn_float64)(nmeaData.altitude);
}//use utm d
/*
vel[0] = vel_data[0];
vel[1] = vel_data[1];
vel[2] = vel_data[2];
*/
//#ifdef VERBOSE
printf("GPS pos: %f, %f, %f\n",pos[0],pos[1],pos[2]);
//#endif
nmeaParser.reset();
//send report -eb
//-----------------------------
//printf("tracker report ready\n",status);
//vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
/*
// Send the message on the connection
if (NULL != vrpn_Tracker::d_connection)
{
char msgbuf[1000];
fprintf(stderr, "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,"GPS: cannot write message: tossing\n");
}
else
{
fprintf(stderr,"packed a message\n\n");
}
}
else
{
fprintf(stderr,"Tracker Fastrak: No valid connection\n");
}
//-----------------------------*/
//printf("%s\n", buffer);
//printf("before first sync status is %d\n",status);
status = vrpn_TRACKER_SYNCING;
//printf("after first set sync status is %d\n",status);
return 1;
}
//no valid alt probably RMC, safe to ignore if we care about alt
} else {//valid lat lon alt fail
printf("GPS cannot determine position (empty fields). Wait for satellites or reposition GPS.\n");
//printf("GPS cannot determine position (empty fields).");
status = vrpn_TRACKER_SYNCING;
return 0;
}
} else {//valid sentence fail
//status = vrpn_TRACKER_FAIL;
status = vrpn_TRACKER_RESETTING;
//is this a good place to put it? If it's not a valid sentence? maybe it should be in reset.
printf("Sentence Invalid. Resetting tracker and resyncing.\n");
return 0;
}
// failed valid sentence
status = vrpn_TRACKER_SYNCING;
return 0;
//#ifdef VERBOSE2
// print_latest_report();
//#endif
}
