vrpn_int32 vrpn_Sound_Client::setSoundPose(const vrpn_SoundID id, vrpn_float64 position[3], vrpn_float64 orientation[4])
{
char buf[sizeof(vrpn_PoseDef) + sizeof(vrpn_SoundID)];
vrpn_int32 len;
vrpn_PoseDef tempdef;
int i;
for (i=0; i<4; i++) {
tempdef.orientation[i] = orientation[i];
}
for (i=0; i<3; i++) {
tempdef.position[i] = position[i];
}
len = encodeSoundPose(tempdef, id, buf);
vrpn_gettimeofday(×tamp, NULL);
if (vrpn_Sound::d_connection->pack_message(len, timestamp, set_sound_pose, d_sender_id, buf, vrpn_CONNECTION_RELIABLE)) {
fprintf(stderr,"vrpn_Sound_Client: cannot write message change status: tossing\n");
}
return 0;
}
void vrpn_SharedObject::becomeSerializer (void) {
timeval now;
// Make sure only one request is outstanding
if (d_isNegotiatingSerializer) {
return;
}
d_isNegotiatingSerializer = vrpn_TRUE;
// send requestSerializer
if (d_connection) {
vrpn_gettimeofday(&now, NULL);
d_connection->pack_message(0, d_lastUpdate,
d_requestSerializer_type, d_myId,
NULL, vrpn_CONNECTION_RELIABLE);
}
//fprintf(stderr, "sent requestSerializer\n");
}
int vrpn_ForceDeviceServer::handle_setObjectOrientation_message(
void *userdata, vrpn_HANDLERPARAM p)
{
vrpn_ForceDeviceServer *me = (vrpn_ForceDeviceServer *)userdata;
vrpn_int32 objNum;
vrpn_float32 axis[3];
vrpn_float32 angle;
decode_objectOrientation(p.buffer, p.payload_len, &objNum, axis, &angle);
#ifdef VRPN_USE_HDAPI
struct timeval now;
vrpn_gettimeofday(&now, NULL);
me->send_text_message("Trimesh not supported under HDAPI", now,
vrpn_TEXT_ERROR);
return 0;
#else
me->setObjectOrientation(objNum, axis, angle);
return 0;
#endif
}
vrpn_Analog_5dtUSB::vrpn_Analog_5dtUSB(vrpn_HidAcceptor *filter,
int num_sensors,
bool isLeftHand,
const char *name,
vrpn_Connection *c) :
vrpn_Analog(name, c),
vrpn_HidInterface(filter),
_isLeftHand(isLeftHand),
_wasConnected(false) {
if (num_sensors != 5 && num_sensors != 14) {
throw std::logic_error("The vrpn_Analog_5dtUSB driver only supports 5 or 14 sensors, and a different number was passed!");
}
vrpn_Analog::num_channel = num_sensors;
// Initialize the state of all the analogs
memset(channel, 0, sizeof(channel));
memset(last, 0, sizeof(last));
// Set the timestamp
vrpn_gettimeofday(&_timestamp, NULL);
}
void send_poser_once_in_a_while( void )
{
static long secs = 0;
struct timeval now;
static vrpn_float64 p[3] = { 0, 0, 0 };
static vrpn_float64 dp[3] = {0, 0, 0 };
static vrpn_float64 q[4] = { 1, 1, 1, 1 };
static int count = 0;
static bool doRelative = true;
vrpn_gettimeofday( &now, NULL );
if( secs == 0 )
{ // First time through
secs = now.tv_sec;
}
if( now.tv_sec - secs >= 1 )
{
if( !doRelative )
{
// do a pose request
p[count%3] += 1;
if( p[count%3] > 1 ) p[count%3] = -1;
rposer->request_pose( now, p, q );
count++;
doRelative = true;
}
else
{ // do a relative pose request
dp[count%3] = 0.25;
dp[(count+1)%3] = 0;
dp[(count+2)%3] = 0;
rposer->request_pose_relative( now, dp, q );
doRelative = false;
}
secs = now.tv_sec;
}
}
// (RDK) serial mouse wired up as button device
vrpn_Button_SerialMouse::vrpn_Button_SerialMouse(const char *name,vrpn_Connection *c,
const char *port, int baud, vrpn_MOUSETYPE type)
: vrpn_Button_Filter(name, c)
{
status = BUTTON_FAIL;
printed_error = false;
// Find out the port name and baud rate;
if (port == NULL) {
fprintf(stderr,"vrpn_Button_SerialMouse: NULL port name\n");
return;
} else {
strncpy(portname, port, sizeof(portname));
portname[sizeof(portname)-1] = '\0';
}
num_buttons = 3;
baudrate = baud;
// Open the serial port we are going to use
if ( (serial_fd=vrpn_open_commport(portname, baudrate)) == -1) {
fprintf(stderr,"vrpn_Button_SerialMouse: Cannot open serial port\n");
return;
}
for (vrpn_int32 i = 0; i < num_buttons; i++) {
buttons[i] = lastbuttons[i] = 0;
buttonstate[i] = vrpn_BUTTON_MOMENTARY;
}
mousetype = type;
lastL = lastR = 0;
// first time in read(), this will get set to 0
lastM = (mousetype == THREEBUTTON_EMULATION)?1:0;
// Say we are ready and find out what time it is
status = BUTTON_READY;
vrpn_gettimeofday(×tamp, NULL);
}
void vrpn_Analog_5dtUSB::on_data_received(size_t bytes, vrpn_uint8 *buffer) {
if (bytes != 64) {
std::ostringstream ss;
ss << "Received a too-short report: " << bytes;
struct timeval ts;
vrpn_gettimeofday(&ts, NULL);
send_text_message(ss.str().c_str(), ts, vrpn_TEXT_WARNING);
return;
}
// Decode all full reports.
const float scale = 1.0f / 4096.0f;
vrpn_uint8 * bufptr = buffer;
for (size_t i = 0; i < 16; i++) {
_rawVals[i] = vrpn_unbuffer<vrpn_int16>(bufptr) * scale;
}
switch (vrpn_Analog::num_channel) {
case 5:
for (size_t i = 0; i < 5; ++i) {
channel[i] = _rawVals[i * 3];
// Report this event before parsing the next.
report_changes();
}
break;
case 14:
for (size_t i = 0; i < 14; ++i) {
channel[i] = _rawVals[i];
// Report this event before parsing the next.
report_changes();
}
break;
default:
std::cerr << "Internal error - should not happen: Unrecognized number of channels!" << std::endl;
}
}
// static
int vrpn_SharedObject::handle_requestSerializer (void * userdata,
vrpn_HANDLERPARAM) {
vrpn_SharedObject * s = (vrpn_SharedObject *) userdata;
timeval now;
if (!s->isSerializer() || s->d_isNegotiatingSerializer) {
// ignore this
// we should probably return failure or error?
return 0;
}
s->d_isNegotiatingSerializer = vrpn_TRUE;
if (s->d_connection) {
// Don't set d_isSerializer to FALSE until they've assumed it.
// Until then, retain the serializer status but queue all of our
// messages; when they finish becoming the serializer, we
// set our flag to false and send the queue of set()s we've
// received to them.
// send grantSerializer
vrpn_gettimeofday(&now, NULL);
s->d_connection->pack_message(0, s->d_lastUpdate,
s->d_grantSerializer_type, s->d_myId,
NULL, vrpn_CONNECTION_RELIABLE);
}
// start queueing set()s
s->d_queueSets = vrpn_TRUE;
//fprintf(stderr, "sent grantSerializer\n");
return 0;
}
void vrpn_Mutex_Remote::requestIndex(void)
{
timeval now;
vrpn_int32 buflen = sizeof(vrpn_int32) + sizeof(vrpn_uint32);
char *buf = new char[buflen];
char *bufptr = buf;
vrpn_int32 len = buflen;
vrpn_uint32 ip_addr = getmyIP();
#ifdef _WIN32
vrpn_int32 pid = _getpid();
#else
vrpn_int32 pid = getpid();
#endif
vrpn_buffer(&bufptr, &len, ip_addr);
vrpn_buffer(&bufptr, &len, pid);
#ifdef VERBOSE
printf("requesting index for %lu, %d\n", ip_addr, pid);
#endif
vrpn_gettimeofday(&now, NULL);
d_connection->pack_message(buflen, now, d_requestIndex_type, d_myId, buf,
vrpn_CONNECTION_RELIABLE);
delete[] buf;
return;
}
int vrpn_IMU_Magnetometer::setup_vector(vrpn_IMU_Vector *vector)
{
// Set a default time of now.
struct timeval now;
vrpn_gettimeofday(&now, NULL);
vector->time = now;
// If the name is empty, we're done.
if (vector->params.name.size() == 0) { return 0; }
// Open the analog device and point the remote at it.
// If the name starts with the '*' character, use the server
// connection rather than making a new one.
if (vector->params.name[0] == '*') {
vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str()+1,
d_connection);
#ifdef VERBOSE
printf("vrpn_IMU_Magnetometer: Adding local analog %s\n",
vector->params.name.c_str()+1);
#endif
} else {
vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str());
#ifdef VERBOSE
printf("vrpn_IMU_Magnetometer: Adding remote analog %s\n",
vector->params.name.c_str());
#endif
}
if (vector->ana == NULL) {
fprintf(stderr,"vrpn_IMU_Magnetometer: "
"Can't open Analog %s\n", vector->params.name.c_str());
return -1;
}
// Set up the callback handler for the channel
return vector->ana->register_change_handler(vector, handle_analog_update);
}
int vrpn_ForceDeviceServer::handle_updateTrimeshChanges_message(
void *userdata, vrpn_HANDLERPARAM p)
{
vrpn_ForceDeviceServer *me = (vrpn_ForceDeviceServer *)userdata;
float SurfaceKspring, SurfaceKdamping, SurfaceFdynamic, SurfaceFstatic;
vrpn_int32 objNum;
decode_updateTrimeshChanges(p.buffer, p.payload_len, &objNum,
&SurfaceKspring, &SurfaceKdamping,
&SurfaceFdynamic, &SurfaceFstatic);
#ifdef VRPN_USE_HDAPI
struct timeval now;
vrpn_gettimeofday(&now, NULL);
me->send_text_message("Trimesh not supported under HDAPI", now,
vrpn_TEXT_ERROR);
return 0;
#else
me->updateTrimeshChanges(objNum, SurfaceKspring, SurfaceFstatic,
SurfaceFdynamic, SurfaceKdamping);
return 0;
#endif
}
int vrpn_ForceDeviceServer::handle_setTriangle_message(void *userdata,
vrpn_HANDLERPARAM p)
{
vrpn_ForceDeviceServer *me = (vrpn_ForceDeviceServer *)userdata;
vrpn_int32 triNum, v0, v1, v2, n0, n1, n2;
vrpn_int32 objNum;
decode_triangle(p.buffer, p.payload_len, &objNum, &triNum, &v0, &v1, &v2,
&n0, &n1, &n2);
#ifdef VRPN_USE_HDAPI
struct timeval now;
vrpn_gettimeofday(&now, NULL);
me->send_text_message("Trimesh not supported under HDAPI", now,
vrpn_TEXT_ERROR);
return 0;
#else
if (me->setTriangle(objNum, triNum, v0, v1, v2, n0, n1, n2))
return 0;
else {
fprintf(stderr, "vrpn_Phantom: error in trimesh::setTriangle\n");
return -1;
}
#endif
}
int vrpn_ForceDeviceServer::handle_addObjectExScene_message(void *userdata,
vrpn_HANDLERPARAM p)
{
vrpn_ForceDeviceServer *me = (vrpn_ForceDeviceServer *)userdata;
vrpn_int32 objNum;
decode_addObjectExScene(p.buffer, p.payload_len, &objNum);
#ifdef VRPN_USE_HDAPI
struct timeval now;
vrpn_gettimeofday(&now, NULL);
me->send_text_message("Trimesh not supported under HDAPI", now,
vrpn_TEXT_ERROR);
return 0;
#else
if (me->addObjectExScene(objNum)) {
return 0;
}
else {
fprintf(stderr, "vrpn_Phantom: error in trimesh::addObjectExScene\n");
return -1;
}
#endif
}
void DeviceThread::AddReport(
std::vector<double> values
, struct timeval sampleTime)
{
// The arrival time is always now.
struct timeval arrivalTime;
vrpn_gettimeofday(&arrivalTime, NULL);
// If the sampleTime is NOW, then replace it with the arrival time.
if ( (sampleTime.tv_sec == NOW.tv_sec) && (sampleTime.tv_usec == NOW.tv_usec)) {
sampleTime = arrivalTime;
}
// This is called in the sub-thread to add a report to the list of available
// reports. We yank the report-vector semaphore while we're adding it to
// avoid races.
DeviceThreadReport r;
r.arrivalTime = arrivalTime;
r.sampleTime = sampleTime;
r.values = values;
m_reportSemaphore.p();
m_reports.push_back(r);
m_reportSemaphore.v();
}
void vrpn_Tracker_WiimoteHead::mainloop() {
struct timeval now;
// Call generic server mainloop, since we are a server
server_mainloop();
// Mainloop() the wiimote to get fresh values
if (d_ana != NULL) {
d_ana->mainloop();
}
// See if we have new data, or if it has been too long since our last
// report. Send a new report in either case.
vrpn_gettimeofday(&now, NULL);
double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
if (_should_report(interval)) {
// Figure out the new matrix based on the current values and
// the length of the interval since the last report
update_pose();
report();
}
}
void vrpn_Dial_Example_Server::mainloop()
{
struct timeval current_time;
int i;
// Call the generic server mainloop, since we are a server
server_mainloop();
// See if its time to generate a new report
// IN A REAL SERVER, this check would not be done; although the
// time of the report would be updated to the current time so
// that the correct timestamp would be issued on the report.
vrpn_gettimeofday(¤t_time, NULL);
if (vrpn_TimevalDuration(current_time, timestamp) >=
1000000.0 / _update_rate) {
// Update the time
timestamp.tv_sec = current_time.tv_sec;
timestamp.tv_usec = current_time.tv_usec;
// Update the values for the dials, to say that each one has
// moved the appropriate rotation (spin rate is revolutions per
// second, update rate is report/second, the quotient is the number
// of revolutions since the last report). When the changes are
// reported, these values are set back to zero.
// THIS CODE WILL BE REPLACED by the user code that tells how
// many revolutions each dial has changed since the last report.
for (i = 0; i < num_dials; i++) {
dials[i] = _spin_rate / _update_rate;
}
// Send reports. Stays the same in a real server.
report_changes();
}
}
void vrpn_Griffin_PowerMate::mainloop(void)
{
update();
server_mainloop();
struct timeval current_time;
vrpn_gettimeofday(¤t_time, NULL);
if (vrpn_TimevalDuration(current_time, _timestamp) > POLL_INTERVAL ) {
_timestamp = current_time;
report_changes();
if (vrpn_Analog::num_channel > 0)
{
vrpn_Analog::server_mainloop();
}
if (vrpn_Button::num_buttons > 0)
{
vrpn_Button::server_mainloop();
}
if (vrpn_Dial::num_dials > 0)
{
vrpn_Dial::server_mainloop();
}
}
}
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
}
int vrpn_Tracker_3Space::get_report(void)
{
int ret;
// The reports are each 20 characters long, and each start with a
// byte that has the high bit set and no other bytes have the high
// bit set. If we're synching, read a byte at a time until we find
// one with the high 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;
}
// If the high bit isn't set, we don't want it we
// need to look at the next one, so just return
if ( (buffer[0] & 0x80) == 0) {
send_text_message("Syncing (high bit not set)", timestamp, vrpn_TEXT_WARNING);
return 0;
}
// Got the first character of a report -- go into PARTIAL mode
// and say that we got one character at this time.
bufcount = 1;
vrpn_gettimeofday(×tamp, NULL);
status = vrpn_TRACKER_PARTIAL;
}
// Read as many bytes of this 20 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],
20-bufcount);
if (ret == -1) {
send_text_message("Error reading, resetting", timestamp, vrpn_TEXT_ERROR);
status = vrpn_TRACKER_FAIL;
return 0;
}
bufcount += ret;
if (bufcount < 20) { // Not done -- go back for more
return 0;
}
{ // Decode the report
unsigned char decode[17];
int i;
static unsigned char mask[8] = {0x01, 0x02, 0x04, 0x08,
0x10, 0x20, 0x40, 0x80 };
// Clear the MSB in the first byte
buffer[0] &= 0x7F;
// Decode the 3Space binary representation into standard
// 8-bit bytes. This is done according to page 4-4 of the
// 3Space user's manual, which says that the high-order bits
// of each group of 7 bytes is packed into the 8th byte of the
// group. Decoding involves setting those bits in the bytes
// iff their encoded counterpart is set and then skipping the
// byte that holds the encoded bits.
// We decode from buffer[] into decode[] (which is 3 bytes
// shorter due to the removal of the bit-encoding bytes).
// decoding from buffer[0-6] into decode[0-6]
for (i=0; i<7; i++) {
decode[i] = buffer[i];
if ( (buffer[7] & mask[i]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[8-14] into decode[7-13]
for (i=7; i<14; i++) {
decode[i] = buffer[i+1];
if ( (buffer[15] & mask[i-7]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// decoding from buffer[16-18] into decode[14-16]
for (i=14; i<17; i++) {
decode[i] = buffer[i+2];
if ( (buffer[19] & mask[i-14]) != 0) {
decode[i] |= (unsigned char)(0x80);
}
}
// Parse out sensor number, which is the second byte and is
// stored as the ASCII number of the sensor, with numbers
// starting from '1'. We turn it into a zero-based unit number.
d_sensor = decode[1] - '1';
// Position
for (i=0; i<3; i++) {
pos[i] = (* (short*)(&decode[3+2*i])) * T_3_BINARY_TO_METERS;
}
// Quarternion orientation. The 3Space gives quaternions
// as w,x,y,z while the VR code handles them as x,y,z,w,
// so we need to switch the order when decoding. Also the
// tracker does not normalize the quaternions.
d_quat[3] = (* (short*)(&decode[9]));
for (i=0; i<3; i++) {
d_quat[i] = (* (short*)(&decode[11+2*i]));
}
//Normalize quaternion
double norm = sqrt ( d_quat[0]*d_quat[0] + d_quat[1]*d_quat[1]
+ d_quat[2]*d_quat[2] + d_quat[3]*d_quat[3]);
for (i=0; i<4; i++) {
d_quat[i] /= norm;
}
// Done with the decoding, set the report to ready
// Ready for another report
status = vrpn_TRACKER_SYNCING;
bufcount = 0;
}
return 1; // Got a report.
#ifdef VERBOSE
print_latest_report();
#endif
}
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_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
}
bool vrpn_Imager_Stream_Buffer::make_new_logging_connection(
const char *local_in_logfile_name, const char *local_out_logfile_name,
const char *remote_in_logfile_name, const char *remote_out_logfile_name)
{
// Open a new connection to do logging on before deleting the old one so
// that we keep at least one connection open to the server at all time.
// This will prevent it from doing its "dropped last connection" things
// which will include resetting the imager server.
vrpn_Connection *new_log_connection = open_new_log_connection(
local_in_logfile_name, local_out_logfile_name, remote_in_logfile_name,
remote_out_logfile_name);
if (new_log_connection == NULL) {
fprintf(stderr, "vrpn_Imager_Stream_Buffer::make_new_logging_"
"connection(): Cannot open connection\n");
return false;
}
// Unhook the callbacks from the existing logging connection so that
// we don't end up with two callbacks for each message.
if (!teardown_handlers_for_logging_connection(d_log_connection)) {
fprintf(stderr, "vrpn_Imager_Stream_Buffer::make_new_logging_"
"connection(): Cannot teardown connection\n");
return false;
}
// Hook the callbacks up to the new connection so that we will get reports
// from the server.
if (!setup_handlers_for_logging_connection(new_log_connection)) {
fprintf(stderr, "vrpn_Imager_Stream_Buffer::make_new_logging_"
"connection(): Cannot setup connection\n");
return false;
}
// Mainloop the new connection object until it becomes connected or we
// time out. If we time out, then put things back on the old connection
// and tell the thread it is time to self-destruct. The way we check
// for connected cannot be just that the connection's connected() method
// returns true (because our end can be marked connected before the other
// end decides it has complete the connection. Rather, we check to see
// that we've got a new description report from the server -- indicating
// that it has seen the new report. This also lets us know that the old
// log file will have accumulated all images up to the new report, so we
// can shut it off without losing any images in the switch to the new
// log file (there may be duplicates, but not losses).
struct timeval start, now;
vrpn_gettimeofday(&start, NULL);
now = start;
d_ready_to_drop_old_connection = false;
while (!d_ready_to_drop_old_connection &&
(vrpn_TimevalDiff(now, start).tv_sec < 3)) {
new_log_connection->mainloop(); // Enable connection set-up to occur
new_log_connection->save_log_so_far();
d_log_connection->mainloop(); // Eat up (and log) any incoming messages
d_log_connection->save_log_so_far();
vrpn_gettimeofday(&now, NULL);
vrpn_SleepMsecs(1);
};
if (!d_ready_to_drop_old_connection) {
fprintf(stderr, "vrpn_Imager_Stream_Buffer::make_new_logging_"
"connection(): Could not connect new logging "
"connection\n");
teardown_handlers_for_logging_connection(new_log_connection);
setup_handlers_for_logging_connection(d_log_connection);
new_log_connection->removeReference();
d_shared_state.time_to_exit(true);
return false;
}
// Delete the old connection object by reducing its reference count.
d_log_connection->removeReference();
// Set up to use the new connection
d_log_connection = new_log_connection;
return true;
}
int main (int, char **) {
vrpn_Connection * connection;
connection = vrpn_create_server_connection(4999);
int myID = connection->register_sender("rpc_Test");
rpc_Test * enc_out = new rpc_Test(connection);
connection->mainloop();
struct timeval now;
char * buf = NULL;
vrpn_int32 len = 0;
vrpn_gettimeofday(&now, NULL);
printf("Expect Empty\n");
connection->pack_message(len, now, enc_out->d_Empty_type, myID,
(char *) buf, vrpn_CONNECTION_RELIABLE);
connection->mainloop();
buf = enc_out->encode_Simple(&len, 2, 1.85f, 3.23f);
vrpn_gettimeofday(&now, NULL);
printf("Expect Simple, 2, 1.85, 3.23\n");
connection->pack_message(len, now, enc_out->d_Simple_type, myID,
(char *) buf, vrpn_CONNECTION_RELIABLE);
delete [] buf; buf = NULL;
connection->mainloop();
const vrpn_int32 cnt = 3;
char nm[NAME_LENGTH] = "Jones";
char nm2[cnt]= "nM";
char doublenm[4][NAME_LENGTH] = {"one", "two", "three", "four"};
int i;
// char *p_to_char[4]= {"one", "two", "three", "four"};
// char (*arr_of_p)[4] ;
// printf ("size %d %d\n", sizeof(p_to_char), sizeof(arr_of_p));
char **triplenm[4]; //char **(*p_t)[4] = &triplenm;
for (i =0; i<4; i++) {
triplenm[i] = new char *[cnt];
for (int j=0; j< cnt; j++) {
triplenm[i][j] = new char[cnt];
sprintf (triplenm[i][j], "%c%c", (char)((int)'a' + i),
(char)((int)'j' + j));
}
}
// char triplenm[4][cnt][cnt] = { {"aa", "ab", "ac" } ,
// {"ba", "bb", "bc" } ,
// {"ca", "cb", "cc" } ,
// {"da", "db", "dc" } };
buf = enc_out->encode_CharArray(&len, cnt, nm, nm2,
doublenm, triplenm);
vrpn_gettimeofday(&now, NULL);
printf("Expect CharArray ...\n");
connection->pack_message(len, now, enc_out->d_CharArray_type, myID,
(char *) buf, vrpn_CONNECTION_RELIABLE);
delete [] buf; buf = NULL;
connection->mainloop();
// vrpn_int32 cnt;
vrpn_int32 shortstuff[] = {1,2,3};
vrpn_int32 constdouble[6][4];
vrpn_int32 *triple[4][NAME_LENGTH];
for ( i =0; i<6; i++) {
for (int j=0; j< 4; j++) {
constdouble[i][j] = i+j;
}
}
for ( i =0; i<4; i++) {
for (int j=0; j< NAME_LENGTH; j++) {
triple[i][j] = new vrpn_int32[cnt];
for (int k=0; k< cnt; k++) {
triple[i][j][k] = i+j+k;
}
}
}
buf = enc_out->encode_IntArray(&len, cnt, &(shortstuff[0]),
constdouble, triple);
vrpn_gettimeofday(&now, NULL);
printf("Expect IntArray ...\n");
connection->pack_message(len, now, enc_out->d_IntArray_type, myID,
(char *) buf, vrpn_CONNECTION_RELIABLE);
delete [] buf; buf = NULL;
connection->mainloop();
vrpn_int32 count = 3;
char name[64] = "Z Piezo";
char units[64]= "nm";
vrpn_float32 offset = 1.3f;
vrpn_float32 scale = 0.025f;
char * mptr; int mlen;
buf = enc_out->encode_ReportScanDatasets_header ( &len, &mptr, &mlen, count );
for (int lv_1 = 0; lv_1 < count; lv_1++) {
enc_out->encode_ReportScanDatasets_body ( &len, buf, &mptr, &mlen,
name, units, offset, scale );
offset *= 2;
scale *=3;
}
vrpn_gettimeofday(&now, NULL);
printf("Expect ReportScanDatasets ...\n");
connection->pack_message(len, now, enc_out->d_ReportScanDatasets_type, myID,
(char *) buf, vrpn_CONNECTION_RELIABLE);
delete [] buf; buf = NULL;
connection->mainloop();
return 0;
}
bool vrpn_test_threads_and_semaphores(void)
{
//------------------------------------------------------------
// Make a semaphore to test in single-threaded mode. First run its count all the way
// down to zero, then bring it back to the full complement and then bring it down
// again. Check that all of the semaphores are available and also that there are no
// more than expected available.
const unsigned sem_count = 5;
vrpn_Semaphore s(sem_count);
unsigned i;
for (i = 0; i < sem_count; i++) {
if (s.condP() != 1) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore ran out of counts\n");
return false;
}
}
if (s.condP() != 0) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore had too many counts\n");
return false;
}
for (i = 0; i < sem_count; i++) {
if (s.v() != 0) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Could not release Semaphore\n");
return false;
}
}
for (i = 0; i < sem_count; i++) {
if (s.condP() != 1) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore ran out of counts, round 2\n");
return false;
}
}
if (s.condP() != 0) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore had too many counts, round 2\n");
return false;
}
//------------------------------------------------------------
// Get a semaphore and use it to construct a thread data structure and then
// a thread. Use that thread to test whether threading is enabled (if not, then
// this completes our testing) and to find out how many processors there are.
vrpn_ThreadData td;
td.pvUD = NULL;
vrpn_Thread t(vrpn_test_thread_body, td);
// If threading is not enabled, then we're done.
if (!t.available()) {
return true;
}
// Find out how many processors we have.
unsigned num_procs = t.number_of_processors();
if (num_procs == 0) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): vrpn_Thread::number_of_processors() returned zero\n");
return false;
}
//------------------------------------------------------------
// Now make sure that we can actually run a thread. Do this by
// creating a semaphore with one entry and calling p() on it.
// Then make sure we can't p() it again and then run a thread
// that will call v() on it when it runs.
vrpn_Semaphore sem;
if (sem.p() != 1) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): thread-test Semaphore had no count\n");
return false;
}
if (sem.condP() != 0) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): thread-test Semaphore had too many counts\n");
return false;
}
t.userData(&sem);
if (!t.go()) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Could not start thread\n");
return false;
}
struct timeval start;
struct timeval now;
vrpn_gettimeofday(&start, NULL);
while (true) {
if (sem.condP() == 1) {
// The thread must have run; we got the semaphore!
break;
}
// Time out after three seconds if we haven't had the thread run to reset
// the semaphore.
vrpn_gettimeofday(&now, NULL);
struct timeval diff = vrpn_TimevalDiff( now, start );
if (diff.tv_sec >= 3) {
fprintf(stderr, "vrpn_test_threads_and_semaphores(): Thread didn't run\n");
return false;
}
vrpn_SleepMsecs(1);
}
return true;
}
int vrpn_gettimeofday(timeval *tp, void *voidp)
{
static int fFirst=1;
static int fHasPerfCounter=1;
static struct _timeb tbInit;
static LARGE_INTEGER liInit;
static LARGE_INTEGER liNow;
static LARGE_INTEGER liDiff;
timeval tvDiff;
#ifndef _STRUCT_TIMEZONE
#define _STRUCT_TIMEZONE
/* from HP-UX */
struct timezone {
int tz_minuteswest; /* minutes west of Greenwich */
int tz_dsttime; /* type of dst correction */
};
#endif
struct timezone *tzp = (struct timezone *)voidp;
if (!fHasPerfCounter) {
_ftime(&tbInit);
tp->tv_sec = tbInit.time;
tp->tv_usec = tbInit.millitm*1000;
return 0;
}
if (fFirst) {
LARGE_INTEGER liTemp;
// establish a time base
fFirst=0;
// Check to see if we are on a Windows NT machine (as opposed to a
// Windows 95/98 machine). If we are not, then use the _ftime code
// because the hi-perf clock does not work under Windows 98SE on my
// laptop, although the query for one seems to indicate that it is
// available.
{ OSVERSIONINFO osvi;
memset(&osvi, 0, sizeof(OSVERSIONINFO));
osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);
GetVersionEx(&osvi);
if (osvi.dwPlatformId != VER_PLATFORM_WIN32_NT) {
fprintf(stderr, "\nvrpn_gettimeofday: disabling hi performance clock on non-NT system. "
"Defaulting to _ftime (~6 ms resolution) ...\n");
fHasPerfCounter=0;
vrpn_gettimeofday( tp, tzp );
return 0;
}
}
// check that hi-perf clock is available
if ( !(fHasPerfCounter = QueryPerformanceFrequency( &liTemp )) ) {
fprintf(stderr, "\nvrpn_gettimeofday: no hi performance clock available. "
"Defaulting to _ftime (~6 ms resolution) ...\n");
fHasPerfCounter=0;
vrpn_gettimeofday( tp, tzp );
return 0;
}
if (vrpn_AdjustFrequency()<0) {
fprintf(stderr, "\nvrpn_gettimeofday: can't verify clock frequency. "
"Defaulting to _ftime (~6 ms resolution) ...\n");
fHasPerfCounter=0;
vrpn_gettimeofday( tp, tzp );
return 0;
}
// get current time
// We assume this machine has a time stamp counter register --
// I don't know of an easy way to check for this
rdtsc( liInit );
_ftime(&tbInit);
// we now consider it to be exactly the time _ftime returned
// (beyond the resolution of _ftime, down to the perfCounter res)
}
// now do the regular get time call to get the current time
rdtsc( liNow );
// find offset from initial value
liDiff.QuadPart = liNow.QuadPart - liInit.QuadPart;
tvDiff.tv_sec = (long) ( liDiff.QuadPart / VRPN_CLOCK_FREQ );
tvDiff.tv_usec = (long)(1e6*((liDiff.QuadPart-VRPN_CLOCK_FREQ
*tvDiff.tv_sec)
/ (double) VRPN_CLOCK_FREQ) );
// pack the value and clean it up
tp->tv_sec = tbInit.time + tvDiff.tv_sec;
tp->tv_usec = tbInit.millitm*1000 + tvDiff.tv_usec;
while (tp->tv_usec >= 1000000) {
tp->tv_sec++;
tp->tv_usec -= 1000000;
}
return 0;
}
if (tzp != NULL) {
TIME_ZONE_INFORMATION tz;
GetTimeZoneInformation(&tz);
tzp->tz_minuteswest = tz.Bias ;
tzp->tz_dsttime = (tz.StandardBias != tz.Bias);
}
return 0;
}
#endif //defined(VRPN_WINDOWS_CLOCK_V2)
#endif //defined(_WIN32)
// do the calibration before the program ever starts up
static timeval __tv;
static int __iTrash = vrpn_gettimeofday(&__tv, (struct timezone *)NULL);
#endif // VRPN_UNSAFE_WINDOWS_CLOCK
#include <stdio.h> // for fprintf, stderr, perror, etc
#include <string.h> // for memcpy, strlen, strcpy, etc
#ifndef _WIN32
#include <errno.h> // for EAGAIN, errno
#include <signal.h> // for pthread_kill, SIGKILL
#endif
#define ALL_ASSERT(exp, msg) if(!(exp)){ fprintf(stderr, "\nAssertion failed! \n %s (%s, %s)\n", msg, __FILE__, __LINE__); }
// init all fields in init()
vrpn_Semaphore::vrpn_Semaphore( int cNumResources ) :
// plays at most one entry which comes before end_filetime
// returns
// -1 on error (including EOF, call eof() to test)
// 0 for normal result (played one entry)
// 1 if we hit end_filetime
int vrpn_File_Connection::playone_to_filetime( timeval end_filetime )
{
vrpn_Endpoint * endpoint = d_endpoints[0];
timeval now;
int retval;
// If we don't have a currentLogEntry, then we've gone past the end of the
// file.
if (!d_currentLogEntry) {
return 1;
}
vrpn_HANDLERPARAM & header = d_currentLogEntry->data;
if (vrpn_TimevalGreater(header.msg_time, end_filetime)) {
// there are no entries to play after the current
// but before end_filetime
return 1;
}
// TCH July 2001
// XXX A big design decision: do we re-log messages exactly,
// or do we mark them with the time they were played back?
// Maybe this should be switchable, but the latter is what
// I need yesterday.
vrpn_gettimeofday(&now, NULL);
retval = endpoint->d_inLog->logIncomingMessage
(header.payload_len, now, header.type,
header.sender, header.buffer);
if (retval) {
fprintf(stderr, "Couldn't log \"incoming\" message during replay!\n");
return -1;
}
// advance current file position
d_time = header.msg_time;
// Handle this log entry
if (header.type >= 0) {
#ifdef VERBOSE
printf("vrpn_FC: Msg Sender (%s), Type (%s), at (%ld:%ld)\n",
endpoint->other_senders[header.sender].name,
endpoint->other_types[header.type].name,
header.msg_time.tv_sec, header.msg_time.tv_usec);
#endif
if (endpoint->local_type_id(header.type) >= 0) {
if (do_callbacks_for(endpoint->local_type_id(header.type),
endpoint->local_sender_id(header.sender),
header.msg_time, header.payload_len,
header.buffer)) {
return -1;
}
}
} else { // system handler
if (header.type != vrpn_CONNECTION_UDP_DESCRIPTION) {
if (doSystemCallbacksFor(header, endpoint)) {
fprintf(stderr, "vrpn_File_Connection::playone_to_filename: "
"Nonzero system return.\n");
return -1;
}
}
}
return advance_currentLogEntry();
}
// virtual
int vrpn_File_Connection::mainloop( const timeval * /*timeout*/ )
{
// XXX timeout ignored for now, needs to be added
timeval now_time;
vrpn_gettimeofday(&now_time, NULL);
if ((d_last_time.tv_sec == 0) && (d_last_time.tv_usec == 0)) {
// If first iteration, consider 0 time elapsed
d_last_time = now_time;
d_filetime_accum.reset_at_time( now_time );
return 0;
}
// now_time: current wallclock time (on method entry)
// d_last_time: wallclock time of last call to mainloop
// (juliano-8/26/99) NO! It is the time the
// wallclock read (at the top of mainloop) when the
// last event was played back from the file.
// If you call mainloop frequently enough,
// these are not necessarily the same!
// (may call mainloop too soon and then no event
// is played back from the file)
// d_time: current time in file
// end_time: computed time in file
// d_rate: wallclock -> fileclock rate scale factor
// goal: compute end_time, then advance to it
//
// scale elapsed time by d_rate (rate of replay);
// this gives us the time to advance (skip_time)
// our clock to (next_time).
// -- see note above!
//
//const timeval real_elapsed_time // amount of ellapsed wallclock time
// = vrpn_TimevalDiff( now_time, d_last_time );
//const timeval skip_time // scale it by d_rate
// = vrpn_TimevalScale( real_elapsed_time, d_rate );
//const timeval end_time // add it to the last file-time
// = vrpn_TimevalSum( d_time, skip_time );
//
// ------ new way of calculating end_time ------------
d_filetime_accum.accumulate_to( now_time );
const timeval end_time = vrpn_TimevalSum(
d_time, d_filetime_accum.accumulated() );
// (winston) Had to add need_to_play() because at fractional rates
// (even just 1/10th) the d_time didn't accumulate properly
// because tiny intervals after scaling were too small
// for a timeval to represent (1us minimum).
//
// (juliano-8/26/99) if ((end_time - timestamp of next event) < 1us)
// then you have run out of precision in the struct timeval when
// need_to_play differences those two timevals. I.e., they
// appear to be the same time.
// need_to_play will return n:n>1 only if this difference
// is non-zero.
//
// (juliano-8/25/99) need_to_play is not a boolean function!
// it returns n:n>0 if you need to play
// n=0 if the timevals compare equal
// n=-1 if there was an error reading the next event
// from the log file
const int need_to_play_retval = need_to_play(end_time);
if (need_to_play_retval > 0) {
d_last_time = now_time;
d_filetime_accum.reset_at_time( now_time );
const int rv = play_to_filetime(end_time);
return rv;
} else if (need_to_play_retval == 0) {
// (winston) we don't set d_last_time so that we can more
// accurately measure the (larger) interval next time around
//
// (juliano-8/26/99) sounds right. Only set d_last_time
// if you actually played some event from the file.
// You may get here if you have your data in more than one
// file, and are trying to play back from the files in lockstep.
// The tracker group does this to run the hybrid tracking
// algorithm on both an inertial data file and a hiball
// tracker file that were recorded with synchronized clocks.
return 0;
} else {
// return something to indicate there was an error
// reading the file
return -1;
// an error occurred while reading the next event from the file
// let's close the connection.
// XXX(jj) is this the right thing to do?
// XXX(jj) for now, let's leave it how it was
// XXX(jj) come back to this and do it right
/*
fprintf( stderr, "vrpn_File_Connection::mainloop(): error reading "
"next event from file. Skipping to end of file. "
"XXX Please edit this function and fix it. It should probably"
" close the connection right here and now.\n");
d_last_time = now_time;
d_filetime_accum.reset_at_time( now_time );
return play_to_filetime(end_time);
*/
}
}
// virtual
int vrpn_File_Connection::read_entry (void)
{
vrpn_LOGLIST * newEntry;
int retval;
newEntry = new vrpn_LOGLIST;
if (!newEntry) {
fprintf(stderr, "vrpn_File_Connection::read_entry: Out of memory.\n");
return -1;
}
// Only print this message every second or so
if (!d_file) {
static struct timeval last_told = {0,0};
static struct timeval now;
vrpn_gettimeofday(&now, NULL);
if (now.tv_sec != last_told.tv_sec) {
fprintf(stderr, "vrpn_File_Connection::read_entry: no open file\n");
memcpy(&last_told, &now, sizeof(last_told));
}
delete newEntry;
return -1;
}
// Get the header of the next message. This was done as a horrible
// hack in the past, where we read the sizeof a struct from the file,
// including a pointer. This of course changed on 64-bit architectures.
// The pointer value was not needed. We now read it as an array of
// 32-bit values and then stuff these into the structure. Unfortunately,
// we now need to both send and read the bogus pointer value if we want
// to be compatible with old versions of log files.
vrpn_HANDLERPARAM & header = newEntry->data;
vrpn_int32 values[6];
retval = fread(values, sizeof(vrpn_int32), 6, d_file);
// return 1 if nothing to read OR end-of-file;
// the latter isn't an error state
if (retval <= 0) {
// Don't close the file because we might get a reset message...
delete newEntry;
return 1;
}
header.type = ntohl(values[0]);
header.sender = ntohl(values[1]);
header.msg_time.tv_sec = ntohl(values[2]);
header.msg_time.tv_usec = ntohl(values[3]);
header.payload_len = ntohl(values[4]);
header.buffer = NULL; // values[5] is ignored -- it used to hold the bogus pointer.
// get the body of the next message
if (header.payload_len > 0) {
header.buffer = new char [header.payload_len];
if (!header.buffer) {
fprintf(stderr, "vrpn_File_Connection::read_entry: "
"Out of memory.\n");
return -1;
}
retval = fread((char *) header.buffer, 1, header.payload_len, d_file);
}
// return 1 if nothing to read OR end-of-file;
// the latter isn't an error state
if (retval <= 0) {
// Don't close the file because we might get a reset message...
return 1;
}
// If we are accumulating messages, keep the list of them up to
// date. If we are not, toss the old to make way for the new.
// Whenever this function returns 0, we need to have set the
// Head and Tail to something non-NULL.
if (d_accumulate) {
// doubly-linked list maintenance, putting this one at the tail.
newEntry->next = NULL;
newEntry->prev = d_logTail;
if (d_logTail) {
d_logTail->next = newEntry;
}
d_logTail = newEntry;
// If we've not gotten any messages yet, this one is also the
// head.
if (!d_logHead) {
d_logHead = d_logTail;
}
} else { // Don't keep old list entries.
// If we had a message before, get rid of it and its data now. We
// could use either Head or Tail here because they will point
// to the same message.
if (d_logTail) {
if (d_logTail->data.buffer) {
delete [] (char *) d_logTail->data.buffer;
}
delete d_logTail;
}
// This is the only message in memory, so it is both the
// head and the tail of the memory list.
d_logHead = d_logTail = newEntry;
// The new entry is not linked to any others (there are no others)
newEntry->next = NULL;
newEntry->prev = NULL;
}
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;
}
