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;
}
int main (int argc, char ** argv) {
int port = vrpn_DEFAULT_LISTEN_PORT_NO;
vrpn_Connection * connection;
char con_name[1024];
sprintf(con_name, ":%d", port);
connection = vrpn_create_server_connection(con_name, NULL, NULL);
vrpn_Tracker * nt;
if ((nt = new vrpn_Tracker_NULL("Tracker0", connection, 2, 2.0)) == NULL)
{
fprintf(stderr,"Can't create new vrpn_Tracker_NULL\n");
return -1;
}
fprintf(stderr,"Created new NULL Tracker\nUse 'vrpn_print_devices Tracker0@localhost' in vrpn/client_src/pc_linux to see results\n");
while (1) {
nt->mainloop();
connection->mainloop();
// Sleep so we don't eat the CPU
vrpn_SleepMsecs(1);
}
}
void vrpn_Imager_Stream_Buffer::handle_request_logging(
const char *local_in_logfile_name, const char *local_out_logfile_name,
const char *remote_in_logfile_name, const char *remote_out_logfile_name)
{
// Request that the logging thread start new logs.
d_shared_state.set_logfile_request(
local_in_logfile_name, local_out_logfile_name, remote_in_logfile_name,
remote_out_logfile_name);
// Wait until we hear back from the logging thread or time out;
// return empty if timeout and the strings we got back if not.
// Remember to deallocated the memory if we got a response.
struct timeval start, now;
vrpn_gettimeofday(&start, NULL);
do {
char *lil, *lol, *ril, *rol;
if (d_shared_state.get_logfile_result(&lil, &lol, &ril, &rol)) {
send_report_logging(lil, lol, ril, rol);
delete[] lil;
delete[] lol;
delete[] ril;
delete[] rol;
return;
}
vrpn_SleepMsecs(1);
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDiff(now, start).tv_sec < 2);
// Timeout, report failure of logging by saying that there are empty log
// file names.
send_report_logging("", "", "", "");
}
int vrpn_Tracker_Liberty::set_sensor_output_format(int sensor)
{
char outstring[64];
const char *timestring;
const char *buttonstring;
const char *analogstring;
// Set output format for the station to be position and quaternion,
// and any of the extended Liberty (stylus with button) or
// IS900 states (timestamp, button, analog).
// This command is a capitol 'o' followed by the number of the
// station, then comma-separated values (2 for xyz, 7 for quat, 8 for
// timestamp, 10 for buttons, 0 for space) that
// indicate data sets, followed by character 13 (octal 15).
// Note that the sensor number has to be bumped to map to station number.
timestring = ",8";
buttonstring = stylus_buttons[sensor] ? ",10" : "";
analogstring="";
sprintf(outstring, "O%d,2,7%s%s%s,0\015", sensor+1, timestring,
buttonstring, analogstring);
if (VRPN_LIBERTY_DEBUG) fprintf(stderr,"[DEBUG]: %s \n",outstring);
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
VRPN_MSG_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
return 0;
}
void dtkVrFlystickRecognizerPrivate::run(void)
{
#if defined(DTK_HAVE_VRPN)
vrpn_FILE_CONNECTIONS_SHOULD_PRELOAD = false;
vrpn_FILE_CONNECTIONS_SHOULD_ACCUMULATE = false;
this->analog = new vrpn_Analog_Remote(url.toString().toAscii().constData());
this->button = new vrpn_Button_Remote(url.toString().toAscii().constData());
this->tracker = new vrpn_Tracker_Remote(url.toString().toAscii().constData());
if (!this->analog || !this->button || !this->tracker) {
qDebug() << "Error connecting to server";
return;
}
this->button->register_change_handler(this, vrpn_flystick_recognizer_handle_button);
this->analog->register_change_handler(this, vrpn_flystick_recognizer_handle_analog);
this->tracker->register_change_handler(this, vrpn_flystick_recognizer_handle_tracker);
while(this->running) {
this->analog->mainloop();
this->button->mainloop();
this->tracker->mainloop();
vrpn_SleepMsecs(10);
}
delete this->analog;
delete this->button;
delete this->tracker;
#endif
}
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");
}
}
void myIdleFunc(void)
{
//------------------------------------------------------------
// This must be done in any Tcl app, to allow Tcl/Tk to handle
// events
while (Tk_DoOneEvent(TK_DONT_WAIT)) {};
//------------------------------------------------------------
// This is called once every time through the main loop. It
// pushes changes in the C variables over to Tcl.
if (Tclvar_mainloop()) {
fprintf(stderr,"Tclvar Mainloop failed\n");
}
// See if we are done.
if (g_quit) {
cleanup();
exit(0);
}
// See if there are any more messages from the server and then sleep
// a little while so that we don't eat the whole CPU.
g_ti->mainloop();
vrpn_SleepMsecs(5);
}
int main (int argc, char * argv [])
{
unsigned controller = 0, sendBody = 0, sendUser = 1;
sprintf(trackerName, "%s", TRACKER_NAME);
// The only command line argument is a string for the configuration
if (argc > 1) {
// Get the arguments (device_name, controller_index, bodyFrame, userFrame)
if (sscanf(argv[1], "%511s%u%u%u", trackerName, &controller, &sendBody, &sendUser) != 4) {
fprintf(stderr, "Bad vrpn_Freespace line: %s\n", argv[1]);
fprintf(stderr, "Expect: deviceName controllerIndex bodyFrame userFrame\n");
return -1;
}
}
// Correctly handle command line input
addControlHandler();
//---------------------------------------------------------------------
// explicitly open the connection
connection = vrpn_create_server_connection(CONNECTION_PORT);
//---------------------------------------------------------------------
// create a freespace tracker for the first device.
printf("Tracker's name is %s.\n", trackerName);
freespace = vrpn_Freespace::create(trackerName, connection, 0, (sendBody != 0), (sendUser != 0));
if (!freespace) {
fprintf(stderr, "Error opening freespace device: %s\n", trackerName);
return 1;
}
// the freespace device exposes 3 vrpn interfaces. Tracker, buttons,
// and a Dial (scrollwheel)
// libfreespace also reports values like a mouse (dx/dy), though the
// vrpn messages are not currently sent. Considering the client side doesn't
// have a notion of a mouse, but treats it as an analog device, I didn't feel
// compelled to generate the messages, though the additional code is fairly
// straight forward
create_and_link_tracker_remote(trackerName);
create_and_link_button_remote(trackerName);
create_and_link_dial_remote(trackerName);
while ( !quit ) {
// Let the servers, clients and connection do their things
freespace->mainloop();
rtkr->mainloop();
connection->mainloop();
// Sleep for 1ms each iteration so we don't eat the CPU
vrpn_SleepMsecs(1);
}
delete freespace;
delete rtkr;
delete connection;
return 0;
} /* main */
void vrpn_Tracker_Dyna::reset() {
//static int numResets = 0; // How many resets have we tried?;
static char T_PDYN_C_CTL_C[4] ="\003\003\003";
static int T_PDYN_RECORD_LENGTH = 8;
vrpn_write_characters(serial_fd, (unsigned char*)T_PDYN_C_CTL_C, strlen(T_PDYN_C_CTL_C));
vrpn_write_characters(serial_fd,(const unsigned char *) "4", 1); // set to polling mode;
/* pause 1 second to allow the Dynasight buffer to stabilize */
vrpn_SleepMsecs(1000.0*1);
status = get_status();
if ( status != T_OK ) {
/* if no data, tracker probably not connected. just bag it. */
if ( status == T_PDYN_NO_DATA )
{
fprintf(stderr, "vrpn_Tracker_Dyna::reset(): no data (is tracker turned on?)\n");
status = vrpn_TRACKER_RESETTING;
return;
}
}else {
fprintf(stderr, "vrpn_Tracker_Dyna: return valid status report\n");
reportLength = T_PDYN_RECORD_LENGTH;
// set it to continues mode;
/* clear any leftover data */
my_flush();
/* set the Dynasight to continuous mode */
vrpn_write_characters(serial_fd, (unsigned char*)T_PDYN_C_CTL_C, strlen(T_PDYN_C_CTL_C));
//vrpn_write_characters(serial_fd, (const unsigned char *)"V", 1);
vrpn_write_characters(serial_fd, (const unsigned char *)"0", 1);
//T_PDYN_C_CONTINUOUS = "V"
vrpn_SleepMsecs(1000.0*1);
//vrpn_gettimeofday(×tamp, NULL); // Set watchdog now;
timestamp.tv_sec = -1;
status = vrpn_TRACKER_SYNCING; // We are trying for a new reading;
return;
}
}
void vrpn_Imager_Stream_Buffer::handle_got_first_connection(void)
{
// There should be no thread in existence when this call is made.
// If there is, kill it and complain.
if (d_logging_thread->running()) {
struct timeval now;
vrpn_gettimeofday(&now, NULL);
send_text_message(
"handle_got_first_connection: Thread running when it should not be",
now, vrpn_TEXT_ERROR);
d_logging_thread->kill();
return;
}
// Reset the shared state before starting the thread running.
d_shared_state.init();
// Create a thread whose userdata points at the object that
// created it. Then call the start function on that thread and wait
// for its vrpn_Imager_Remote to receive the info from the remote server
// it has connected to. We time out after a few seconds if we don't
// get the response, leaving us with a presumably broken connection
// to the server.
if (!d_logging_thread->go()) {
struct timeval now;
vrpn_gettimeofday(&now, NULL);
send_text_message(
"handle_got_first_connection: Failed to start logging thread", now,
vrpn_TEXT_ERROR);
delete d_logging_thread;
d_logging_thread = NULL;
return;
}
struct timeval start, now;
vrpn_gettimeofday(&start, NULL);
do {
const char *channelBuffer = NULL;
if (d_shared_state.get_imager_description(
d_nRows, d_nCols, d_nDepth, d_nChannels, &channelBuffer)) {
int i;
const char *bufptr = channelBuffer;
for (i = 0; i < d_nChannels; i++) {
d_channels[i].unbuffer(&bufptr);
}
delete[] const_cast<char *>(channelBuffer);
return;
}
vrpn_SleepMsecs(1);
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDiff(now, start).tv_sec < 3);
// Timed out, so we won't be hearing from the server!
vrpn_gettimeofday(&now, NULL);
send_text_message("handle_got_first_connection: Didn't hear from server.",
now, vrpn_TEXT_WARNING);
}
void DeviceThread::StopThread()
{
// Tell our thread it is time to stop running by grabbing its
// semaphore. Wait until it has stopped and then delete it.
m_quit.p();
while (m_thread->running()) {
vrpn_SleepMsecs(1);
}
delete m_thread;
}
int main (int argc, char * argv [])
{
bool sendBody = 0, sendUser = 1;
// painfully simple CL options to turn on/off body/user frame reports
if (argc > 1) {
sendBody = atoi(argv[1]);
if (argc > 2) {
sendUser = atoi(argv[2]);
}
}
//---------------------------------------------------------------------
// explicitly open the connection
connection = vrpn_create_server_connection(CONNECTION_PORT);
//---------------------------------------------------------------------
// Open the tracker server, using this connection, 2 sensors, update 1 times/sec
printf("Tracker's name is %s.\n", TRACKER_NAME);
// create a freespace tracker for the first device.
freespace = vrpn_Freespace::create(TRACKER_NAME, connection, 0, sendBody, sendUser);
if (!freespace) {
fprintf(stderr, "Error opening freespace device\n");
return 1;
}
// the freespace device exposes 3 vrpn interfaces. Tracker, buttons,
// and a Dial (scrollwheel)
// libfreespace also reports values like a mouse (dx/dy), though the
// vrpn messages are not currently sent. Considering the client side doesn't
// have a notion of a mouse, but treats it as an analog device, I didn't feel
// compelled to generate the messages, though the additional code is fairly
// straight forward
create_and_link_tracker_remote();
create_and_link_button_remote();
create_and_link_dial_remote();
/*
* main interactive loop
*/
while ( 1 ) {
// Let the servers, clients and connection do their things
freespace->mainloop();
rtkr->mainloop();
connection->mainloop();
// Sleep for 1ms each iteration so we don't eat the CPU
vrpn_SleepMsecs(1);
}
return 0;
} /* main */
// This routine writes out the characters slowly, so as not to
// overburden the poor Magellan, which seems to choke when a
// bunch of characters are all sent at once.
static int vrpn_write_slowly(int fd, unsigned char *buffer, size_t len, int MsecWait)
{ size_t i;
for (i = 0; i < len; i++) {
vrpn_SleepMsecs(MsecWait);
if (vrpn_write_characters(fd, &buffer[i], 1) != 1) {
return -1;
}
}
return static_cast<int>(len);
}
int vrpn_Tracker_Dyna::get_status()
{
int bytesRead;
unsigned char statusBuffer[256];
my_flush();
/* send request for status record */
vrpn_write_characters(serial_fd,(const unsigned char *) "\021", 1);
vrpn_drain_output_buffer(serial_fd);
vrpn_SleepMsecs(1000.0*2);
/* do non-blocking read of status record */
bytesRead = vrpn_read_available_characters(serial_fd, statusBuffer, 8);
// T_PDYN_STATUS_RECORD_LENGTH =8;
if ( bytesRead == 8 )
{
/* we have correct length- check a few chars to make sure this is a valid
* record
*/
if ( ((statusBuffer[0] & lOOO_OOOO) != lOOO_OOOO) ||
((statusBuffer[1] & lOOO_OOOO) != lOOO_OOOO) )
return(T_ERROR);
/* otherwise, all is well */
return(T_OK);
}
/* if we get here, we either got too much data or not enough */
/* no data means it's probably disconnected or not turned on */
if ( bytesRead == 0 )
{
// fprintf(stderr, "No data\n");
return(T_PDYN_NO_DATA);
}
/* if we got too much data, chances are that it's in continuous mode */
if ( bytesRead > 8)
{
fprintf(stderr, "3\n");
return(T_PDYN_SPEW_MODE);
}
/* if we get here, i have no idea what's going on- could be garbage on the
* serial line, wrong baud rate, or that the Dynasight is flaking out.
*/
return(T_ERROR);
} /* t_pdyn_get_status */
int vrpn_write_slowly(int comm, const unsigned char *buffer, size_t bytes,
int millisec_delay)
{
size_t i;
for (i = 0; i < bytes; i++) {
vrpn_SleepMsecs(millisec_delay);
if (vrpn_write_characters(comm, &buffer[i], 1) != 1) {
return -1;
}
}
return static_cast<int>(bytes);
}
void myIdleFunc(void)
{
// See if there are any more messages from the server and then sleep
// a little while so that we don't eat the whole CPU.
g_imager->mainloop();
vrpn_SleepMsecs(5);
if (g_quit) {
delete g_imager;
if (g_image) {
delete [] g_image;
g_image = NULL;
};
exit(0);
}
}
HRESULT directx_samplegrabber_callback::SampleCB(double time, IMediaSample *sample)
{
// Point the image sample to the media sample we have and then set the flag
// to tell the application it can process it.
imageSample = sample;
imageReady = true;
// Wait until the image has been processed and then return the buffer to the
// filter graph
while (!imageDone && _stayAlive) { vrpn_SleepMsecs(1); }
if (_stayAlive) {
imageDone = false;
}
return S_OK;
}
int vrpn_Tracker_Isotrak::set_sensor_output_format(int /*sensor*/)
{
char outstring[16];
// Set output format for the station to be position, quaternion
// Don't need the space anymore, though
sprintf(outstring, "O2,11\r");
if (vrpn_write_characters(serial_fd, (const unsigned char *)outstring,
strlen(outstring)) == (int)strlen(outstring)) {
vrpn_SleepMsecs(50); // Sleep for a bit to let command run
} else {
VRPN_MSG_ERROR("Write failed on format command");
status = vrpn_TRACKER_FAIL;
return -1;
}
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 main (int argc, char * argv[]) {
if (argc != 3) {
fprintf(stderr, "Usage: %s hostname port\n", argv[0]);
return -1;
}
char * hostname = argv[1];
char * port = argv[2];
int length = strlen(hostname) + strlen(port) + 1; //+ 1 for ':'
char result[length];
bzero(result, length);
strcpy(result, hostname);
strcat(result, ":");
strcat(result, port);
const char * TRACKER_NAME = "Tracker0";
int CONNECTION_PORT = vrpn_DEFAULT_LISTEN_PORT_NO;
vrpn_Connection * connection = vrpn_get_connection_by_name(result);
//create a dummy server
vrpn_Tracker_NULL * null_tracker = new vrpn_Tracker_NULL(TRACKER_NAME, connection, 1, 60.0);
vrpn_Tracker_Remote * tracker = new vrpn_Tracker_Remote(TRACKER_NAME, connection);
tracker->register_change_handler(NULL, handle_data);
while (1) {
null_tracker->mainloop();
tracker->mainloop();
connection->mainloop();
vrpn_SleepMsecs(1000);
}
disconn(socketfd);
return 0;
}
void samplingLoop(void *arg)
{
sgct::SGCTTrackingManager * tmPtr =
reinterpret_cast<sgct::SGCTTrackingManager *>(arg);
double t;
bool running = true;
while(running)
{
t = sgct::Engine::getTime();
for(size_t i=0; i<tmPtr->getNumberOfTrackers(); i++)
{
sgct::SGCTTracker * trackerPtr = tmPtr->getTrackerPtr(i);
if( trackerPtr != NULL )
{
for(size_t j=0; j<trackerPtr->getNumberOfDevices(); j++)
{
if( trackerPtr->getDevicePtr(j)->isEnabled() )
{
if( gTrackers[i].mDevices[j].mSensorDevice != NULL )
gTrackers[i].mDevices[j].mSensorDevice->mainloop();
if( gTrackers[i].mDevices[j].mAnalogDevice != NULL )
gTrackers[i].mDevices[j].mAnalogDevice->mainloop();
if( gTrackers[i].mDevices[j].mButtonDevice != NULL )
gTrackers[i].mDevices[j].mButtonDevice->mainloop();
}
}
}
}
running = tmPtr->isRunning();
tmPtr->setSamplingTime(sgct::Engine::getTime() - t);
// Sleep for 1ms so we don't eat the CPU
vrpn_SleepMsecs(1);
}
}
// Stop the logging thread function, cleanly if possible. Returns true if
// the function stopped cleanly, false if it had to be killed.
bool vrpn_Imager_Stream_Buffer::stop_logging_thread(void)
{
// Set the flag telling the logging thread to stop.
d_shared_state.time_to_exit(true);
// Wait for up to three seconds for the logging thread to die a clean death.
// If it does, return true.
struct timeval start, now;
vrpn_gettimeofday(&start, NULL);
do {
if (!d_logging_thread->running()) {
return true;
}
vrpn_SleepMsecs(1);
vrpn_gettimeofday(&now, NULL);
} while (vrpn_TimevalDiff(now, start).tv_sec < 3);
d_logging_thread->kill();
return false;
}
void vrpn_Analog_USDigital_A2::mainloop()
{
server_mainloop(); // let the server do its stuff
#ifdef VRPN_USE_USDIGITAL
long readErr, readVal ;
// Read the data from the available channels
for (vrpn_uint32 c=0 ; c<(vrpn_uint32) num_channel ; c++)
{
// see if there's really a readable device there.
if (c<_numDevices && _devAddr[c]>=0)
{
readErr = A2GetPosition(_devAddr[c], &readVal) ;
if (readErr)
{
fprintf(stderr,
"vrpn_Analog_USDigital_A2: Error code %d received while reading channel %d.\n",
readErr, c) ;
fprintf(stderr, "vrpn_Analog_USDigital_A2: Attempting to reinitialize SEI bus...") ;
readErr = ResetSEI() ;
if (readErr)
fprintf(stderr, "failed.") ;
fprintf(stderr, "failed.") ;
// don't flood the log, and give the reset time to work
vrpn_SleepMsecs(1000) ;
}
else
channel[c] = (vrpn_float64) readVal ;
}
else
channel[c] = 0 ; // default to 0 for unreadable/unavailable.
} // for
#endif
// Finally, the point of all this, deliver the data
if (_reportChange)
report_changes() ;
else
report() ;
} // mainloop
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;
}
bool vrpn_Tracker_3DMouse::set_filtering_count(int count)
{
char sBuf[16];
sBuf[0] = 0x2a;
sBuf[1] = 0x24;
sBuf[2] = 2;
sBuf[3] = 7;
sBuf[4] = count;
if (vrpn_write_characters(serial_fd, (const unsigned char*)sBuf, 5) == 5)
{
vrpn_SleepMsecs(1000.0*1);
}
else
{
perror(" 3DMouse write filtering count failed");
status = vrpn_TRACKER_FAIL;
return false;
}
return true;
}
// Request some log files and wait up to a second for the report of these
// files. Return true if we got a report and the names match.
bool test_logfile_names(const char *local_in, const char *local_out,
const char *remote_in, const char *remote_out)
{
struct timeval start;
struct timeval now;
// Mark no report and the request logging with the specified
// parameters.
g_got_report = false;
if (!g_logger->send_logging_request(local_in, local_out, remote_in, remote_out)) {
fprintf(stderr, "test_logfile_names: Logging request send failed\n");
return false;
}
// Mainloop the logger for up to five seconds waiting for a response.
// If we don't get a response, this is a failure.
vrpn_gettimeofday(&start, NULL);
do {
g_logger->mainloop();
vrpn_gettimeofday(&now, NULL);
vrpn_SleepMsecs(1);
} while ( !g_got_report && (vrpn_TimevalDurationSeconds(now, start) < 5.0));
if (!g_got_report) {
fprintf(stderr, "test_logfile_names: Timeout waiting for report of logging from server\n");
return false;
}
// Check to see if the names are the same. Return true if they all are.
if ( (strcmp(g_local_in, local_in) == 0) &&
(strcmp(g_local_out, local_out) == 0) &&
(strcmp(g_remote_in, remote_in) == 0) &&
(strcmp(g_remote_out, remote_out) == 0) ) {
return true;
} else {
return false;
}
}
void vrpn_Tracker_NDI_Polaris::switchToHigherBaudRate(const char *port) {
printf("vrpn_Tracker_NDI_Polaris: Switching NDI to higher baud rate, and then reopening com port at higher rate...");
sendCommand("COMM 70000"); // 1.2Mbit (1228739 baud), which requires the NDI USB 2.0 adapter
readResponse();
#ifdef DEBUG
printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
#endif
//if the response is "RESET", try again
if (strncmp("RESET",(char *) latestResponseStr,5)==0) {
//we got a reset, which means the track reset itself (without being commanded too?)
sendCommand("COMM 70000"); // 1.2Mbit, which requires the NDI USB 2.0 adapter
readResponse();
//printf("DEBUG: COMM response: >%s<\n",latestResponseStr);
}
vrpn_SleepMsecs(100); //we should wait 100 msec after getting the OKAY from the NDI before changing the PC's comm rate
vrpn_close_commport(serialFd);
#ifdef _WIN32
serialFd=vrpn_open_commport(port,19200); //19.2k is aliased to 1.2Mbit in the Window's version of the NDI USB virtual com port driver
#else
serialFd=vrpn_open_commport(port,1228739); //should work on linux (UNTESTED!)
#endif
printf("done\n");
}
// This creates a vrpn_Magellan and sets it to reset mode. It opens
// the serial device using the code in the vrpn_Serial_Analog constructor.
// The box seems to autodetect the baud rate when the "T" command is sent
// to it.
vrpn_Magellan::vrpn_Magellan (const char * name, vrpn_Connection * c,
const char * port, int baud, bool altreset):
vrpn_Serial_Analog(name, c, port, baud),
vrpn_Button_Filter(name, c),
_numbuttons(9),
_numchannels(6),
_null_radius(8),
_altreset(altreset)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
vrpn_Analog::num_channel = _numchannels;
// Set the status of the buttons and analogs to 0 to start
clear_values();
// Set the mode to reset
_status = STATUS_RESETTING;
// Wait before the first time we attempt a reset - seems to be a race condition
// with the device needing time between opening of the serial connection and
// receiving the reset commands. (SpaceMouse Plus XT Serial, version 6.60)
vrpn_SleepMsecs(1000);
}
int main(int argc, char **argv)
{
char default_imager[] = "TestImage@localhost";
char *device_name = default_imager;
char *logfile_name = NULL;
int i;
// Parse the command line. If there is one argument, it is the device
// name. If there is a second, it is a logfile name.
if (argc >= 2) {
device_name = argv[1];
}
if (argc >= 3) {
logfile_name = argv[2];
}
if (argc > 3) {
fprintf(stderr, "Usage: %s [device_name [logfile_name]]\n", argv[0]);
exit(-1);
}
// In case we end up loading an video from a file, make sure we
// neither pre-load nor accumulate the images.
vrpn_FILE_CONNECTIONS_SHOULD_PRELOAD = false;
vrpn_FILE_CONNECTIONS_SHOULD_ACCUMULATE = false;
// Say that we've posted a redisplay so that the callback handler
// for endframe doesn't try to post one before glut is open.
g_already_posted = true;
// Create a log file of the video if we've been asked to do logging.
// This has the side effect of having the imager also use this same
// connection, because VRPN maps the same connection name to the
// same one rather than creating a new one.
if (logfile_name) {
g_connection = vrpn_get_connection_by_name(device_name, logfile_name);
}
// Open the Imager client and set the callback
// for new data and for information about the size of
// the image.
printf("Opening %s\n", device_name);
g_imager = new vrpn_Imager_Remote(device_name);
g_imager->register_description_handler(NULL, handle_description_message);
g_imager->register_region_handler(g_imager, handle_region_change);
g_imager->register_discarded_frames_handler(NULL, handle_discarded_frames);
g_imager->register_end_frame_handler(g_imager, handle_end_of_frame);
printf("Waiting to hear the image dimensions...\n");
while (!g_got_dimensions) {
g_imager->mainloop();
vrpn_SleepMsecs(1);
}
// Because the vrpn_Imager server doesn't follow "The VRPN Way" in that
// it will continue to attempt to flood the network with more data than
// can be sent, we need to tell the client's connection to stop handling
// incoming messages after some finite number, to avoid getting stuck down
// in the imager's mainloop() and never returning control to the main
// program. This strange-named function does this for us. If the camera
// is not sending too many messages for the network, this should not have
// any effect.
g_imager->connectionPtr()->Jane_stop_this_crazy_thing(50);
// Allocate memory for the image and clear it, so that things that
// don't get filled in will be black.
if ( (g_image = new unsigned char[g_Xdim * g_Ydim * 3]) == NULL) {
fprintf(stderr,"Out of memory when allocating image!\n");
return -1;
}
for (i = 0; i < g_Xdim * g_Ydim * 3; i++) {
g_image[i] = 0;
}
g_ready_for_region = true;
printf("Receiving images at size %dx%d\n", g_Xdim, g_Ydim);
printf("Press '0'-'9' in OpenGL window to throttle incoming images.\n");
printf("Press '-' to disable throttling.\n");
printf("Press 'a' to enable/disable autoscaling of brightness.\n");
printf("Press 'q' or 'Q' or ESC to quit.\n");
// Initialize GLUT and create the window that will display the
// video -- name the window after the device that has been
// opened in VRPN.
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(g_Xdim, g_Ydim);
glutInitWindowPosition(100, 100);
glutCreateWindow(vrpn_copy_service_name(device_name));
// Set the display function and idle function for GLUT (they
// will do all the work) and then give control over to GLUT.
glutDisplayFunc(myDisplayFunc);
glutIdleFunc(myIdleFunc);
glutKeyboardFunc(myKeyboardFunc);
glutMainLoop();
// Clean up objects and return. This code is never called because
// glutMainLoop() never returns.
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
}
