// If the time is before our current time (or there is no current // time) then reset back to the beginning. Whether or not we did // that, search forwards until we get to or past the time we are // searching for. // newtime is an elapsed time from the start of the file int vrpn_File_Connection::jump_to_time(timeval newtime) { if( d_earliest_user_time_valid ) { d_time = vrpn_TimevalSum( d_earliest_user_time, newtime ); } else { d_time = vrpn_TimevalSum( d_start_time, newtime ); } // XXX get rid of this option - dtm // If the time is earlier than where we are, or if we have // run past the end (no current entry), jump back to // the beginning of the file before searching. if ( !d_currentLogEntry || vrpn_TimevalGreater(d_currentLogEntry->data.msg_time, d_time) ) { reset(); } // Search forwards, as needed. Do not play the messages as they are // passed, just skip over them until we get to a message that has a // time greater than or equal to the one we are looking for. That is, // one whose time is not less than ours. while (!vrpn_TimevalGreater(d_currentLogEntry->data.msg_time, d_time)) { int ret = advance_currentLogEntry(); if ( ret != 0 ) { return 0; // Didn't get where we were going! } } return 1; // Got where we were going! }
bool ArduinoComparer::computeLatency( int arduinoChannel , int deviceChannel , double &outLatencySeconds , bool arrivalTime ) const { // Bogus value in case we have to bail. outLatencySeconds = -10e10; if ( (m_deviceReports.size() == 0) || (m_arduinoReports.size() == 0) ) { return false; } // Compute the start time, which is the lowest time value in either // of the report lists. struct timeval start; if (arrivalTime) { start = m_deviceReports[0].arrivalTime; if (vrpn_TimevalGreater(start, m_arduinoReports[0].arrivalTime)) { start = m_arduinoReports[0].arrivalTime; } } else { start = m_deviceReports[0].sampleTime; if (vrpn_TimevalGreater(start, m_arduinoReports[0].sampleTime)) { start = m_arduinoReports[0].sampleTime; } } // Construct trajectories for both the Arduino and the device. Trajectory arduinoTrajectory(m_arduinoReports, start, arduinoChannel, arrivalTime); Trajectory deviceTrajectory(m_deviceReports, start, deviceChannel, arrivalTime); // Compute the sum of squared differences between the device values and // the expected mapping for the nearest-time Arduino values for a temporal // offset of 0. double minOffset = 0; double minError = computeError(arduinoTrajectory, deviceTrajectory, minOffset); // @todo Turn this from a brute-force search into an optimization routine. for (double offset = -300e-3; offset <= 300e-3; offset += 1e-3) { double err = computeError(arduinoTrajectory, deviceTrajectory, offset); //std::cout << "XXX offset vs. error: " << offset << ", " << err << std::endl; if (err < minError) { minError = err; minOffset = offset; } } outLatencySeconds = minOffset; return true; }
// checks if there is at least one log entry that occurs // between the current d_time and the given filetime // // [juliano 9/24/99] the above comment is almost right // the upper bound of the interval is not open, // but closed at time_we_want_to_play_to. // // this function checks if the next message to play back // from the stream file has a timestamp LESSTHAN OR EQUAL TO // the argument to this function (which is the time that we // wish to play to). If it does, then a pos value is returned // // you can pause playback of a streamfile by ceasing to increment // the value that is passed to this function. However, if the next // message in the streamfile has the same timestamp as the previous // one, it will get played anyway. Pause will not be achieved until // all such messages have been played. // // Beware: make sure you put the correct timestamps on individual // messages when recording them in chunks (batches) // to a time to a streamfile. // int vrpn_File_Connection::need_to_play( timeval time_we_want_to_play_to ) { // This read_entry() call may be useful to test the state, but // it should be the case that d_currentLogEntry is non-NULL except // when we are at the end. This is because read_entry() and the // constructor now both read the next one in when they are finished. if (!d_currentLogEntry) { int retval = read_entry(); if (retval < 0) { return -1; } // error reading from file if (retval > 0) { return 0; } // end of file; nothing to replay d_currentLogEntry = d_logTail; // If read_entry() returns 0, this will be non-NULL } vrpn_HANDLERPARAM & header = d_currentLogEntry->data; // [juliano 9/24/99] is this right? // this is a ">" test, not a ">=" test // consequently, this function keeps returning true until the // next message is timestamped greater. So, if a group of // messages share a timestamp, you cannot pause streamfile // replay anywhere inside the group. // // this is true, but do you ever want to pause in the middle of // such a group? This was a problem prior to fixing the // timeval overflow bug, but now that it's fixed, (who cares?) return vrpn_TimevalGreater( time_we_want_to_play_to, header.msg_time ); }
// virtual void vrpn_RedundantTransmission::mainloop (void) { queuedMessage * qm; queuedMessage ** snitch; timeval now; if (!d_connection) { return; } //fprintf(stderr, "mainloop: %d messages queued.\n", d_numMessagesQueued); vrpn_gettimeofday(&now, NULL); for (qm = d_messageList; qm; qm = qm->next) { if ((qm->remainingTransmissions > 0) && vrpn_TimevalGreater(now, qm->nextValidTime)) { d_connection->pack_message(qm->p.payload_len, qm->p.msg_time, qm->p.type, qm->p.sender, qm->p.buffer, vrpn_CONNECTION_LOW_LATENCY); qm->nextValidTime = vrpn_TimevalSum(now, qm->transmissionInterval); qm->remainingTransmissions--; //fprintf(stderr, "Sending message; " //"%d transmissions remaining at %d.%d int.\n", //qm->remainingTransmissions, qm->transmissionInterval.tv_sec, //qm->transmissionInterval.tv_usec); } } snitch = &d_messageList; qm = *snitch; while (qm) { if (!qm->remainingTransmissions) { *snitch = qm->next; delete [] (char *) qm->p.buffer; delete qm; qm = *snitch; d_numMessagesQueued--; } else { snitch = &qm->next; qm = *snitch; } } if ((d_numMessagesQueued && !d_messageList) || (!d_numMessagesQueued && d_messageList)) { fprintf(stderr, "vrpn_RedundantTransmission::mainloop(): " "serious internal error.\n"); d_numMessagesQueued = 0; d_messageList = NULL; } }
// plays all entries between d_time and end_filetime // returns -1 on error, 0 on success int vrpn_File_Connection::play_to_filetime(const timeval end_filetime) { vrpn_uint32 playback_this_iteration = 0; if (vrpn_TimevalGreater(d_time, end_filetime)) { // end_filetime is BEFORE d_time (our current time in the stream) // so, we need to go backwards in the stream // currently, this is implemented by // * rewinding the stream to the beginning // * playing log messages one at a time until we get to end_filetime reset(); } int ret; while ((ret = playone_to_filetime(end_filetime)) == 0) { // * you get here ONLY IF playone_to_filetime returned 0 // * that means that it played one entry playback_this_iteration++; if ((get_Jane_value() > 0) && (playback_this_iteration >= get_Jane_value())) { // Early exit from the loop // Don't reset d_time to end_filetime return 0; } } if (ret == 1) { // playone_to_filetime finished or EOF no error for us // Set log position to the exact requested ending time, // don't leave it at the last log entry time d_time = end_filetime; ret = 0; } return ret; }
void VRPNTrackerInstance::getAccReport(vrpn_TRACKERACCCB* cpy, timeval* ts, int in_sensor) { TrackerAccList::iterator it; vrpn_TRACKERACCCB* last = NULL; for ( it = acc.begin(); it != acc.end(); it++ ) { vrpn_TRACKERACCCB* curr = *it; if (curr->sensor == in_sensor) { if (ts == NULL) { *cpy = *curr; return; } else if (vrpn_TimevalGreater(*ts,curr->msg_time)) { if (last) { double val = vrpn_TimevalMsecs(vrpn_TimevalDiff(*ts, curr->msg_time))/vrpn_TimevalMsecs(vrpn_TimevalDiff(last->msg_time, curr->msg_time)); cpy->acc[0] = curr->acc[0] + val*(last->acc[0] - curr->acc[0]); cpy->acc[1] = curr->acc[1] + val*(last->acc[1] - curr->acc[1]); cpy->acc[2] = curr->acc[2] + val*(last->acc[2] - curr->acc[2]); q_slerp(cpy->acc_quat, curr->acc_quat, last->acc_quat, val); cpy->acc_quat_dt = curr->acc_quat_dt + val*(last->acc_quat_dt - curr->acc_quat_dt); return; } else { *cpy = *curr; return; } } else last = curr; } } }
void vrpn_File_Connection::find_superlative_user_times( ) { timeval high = {0, 0}; timeval low = {LONG_MAX, 999999L}; // Remember where we were when we asked this question bool retval = store_stream_bookmark( ); if( retval == false ) { #ifdef VERBOSE printf( "vrpn_File_Connection::find_superlative_user_times: didn't successfully save bookmark.\n" ); #endif return; } // Go to the beginning of the file and then run through all // of the messages to find the one with the lowest/highest value reset(); do { if( d_currentLogEntry && (d_currentLogEntry->data.type >= 0) ) { if( vrpn_TimevalGreater( d_currentLogEntry->data.msg_time, high ) ) { high = d_currentLogEntry->data.msg_time; } if( vrpn_TimevalGreater(low, d_currentLogEntry->data.msg_time ) ) { low = d_currentLogEntry->data.msg_time; } } } while( d_currentLogEntry && ( advance_currentLogEntry( ) == 0 ) ); // We have our value. Set it and go back where // we came from, but don't play the records along // the way. retval = return_to_bookmark( ); if( retval == false ) { // oops. we've really screwed things up. fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times messed up the location in the file stream.\n" ); reset( ); return; } if( high.tv_sec != LONG_MIN ) // we found something { d_highest_user_time = high; d_highest_user_time_valid = true; } #ifdef VERBOSE else { fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times: did not find a highest-time user message\n" } #endif if( low.tv_sec != LONG_MAX ) // we found something { d_earliest_user_time = low; d_earliest_user_time_valid = true; } #ifdef VERBOSE else { fprintf( stderr, "vrpn_File_Connection::find_superlative_user_times: did not find an earliest user message\n" } #endif } // end find_superlative_user_times
// 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 vrpn_bool vrpn_Shared_float64::shouldAcceptUpdate (vrpn_float64 newValue, timeval when, vrpn_bool isLocalSet) { // Is this "new" change idempotent? if ((d_mode & VRPN_SO_IGNORE_IDEMPOTENT) && (newValue == d_value)) { return vrpn_FALSE; } // Is this "new" change older than the previous change? if ((d_mode & VRPN_SO_IGNORE_OLD) && !vrpn_TimevalGreater(when, d_lastUpdate)) { // If the timestamps of the new & previous changes are equal: // - if we are the serializer, we can accept the local change // - if we are not the serializer, local updates are to be rejected if( vrpn_TimevalEqual( when, d_lastUpdate ) ) { if( !d_isSerializer && isLocalSet ) { return vrpn_FALSE; } } else return vrpn_FALSE; } // All other clauses of shouldAcceptUpdate depend on serialization if (!(d_mode & VRPN_SO_DEFER_UPDATES)) { return vrpn_TRUE; } // If we're not the serializer, don't accept local set() calls - // forward those to the serializer. Non-local set() calls are // messages from the serializer that we should accept. if (!d_isSerializer) { if (isLocalSet) { yankDeferredUpdateCallbacks(); return vrpn_FALSE; } else { return vrpn_TRUE; } } // We are the serializer. if (isLocalSet) { if (d_policy == vrpn_DENY_LOCAL) { return vrpn_FALSE; } else { return vrpn_TRUE; } } // Are we accepting all updates? if (d_policy == vrpn_ACCEPT) { return vrpn_TRUE; } // Does the user want to accept this one? if ((d_policy == vrpn_CALLBACK) && d_policyCallback && (*d_policyCallback)(d_policyUserdata, newValue, when, this)) { return vrpn_TRUE; } return vrpn_FALSE; }
//virtual vrpn_bool vrpn_Shared_int32::shouldAcceptUpdate (vrpn_int32 newValue, timeval when, vrpn_bool isLocalSet, vrpn_LamportTimestamp *) { //fprintf(stderr, "In vrpn_Shared_int32::shouldAcceptUpdate(%s).\n", d_name); /* // this commented-out code uses Lamport logical clocks, and is // disabled now b/c the implementation of Lamport clocks was // never complete. We use standard timestamps instead. vrpn_bool old, equal; if (t) { old = d_lastLamportUpdate && (*t < *d_lastLamportUpdate); equal = d_lastLamportUpdate && (*t == *d_lastLamportUpdate); } else { old = !vrpn_TimevalGreater(when, d_lastUpdate); equal = vrpn_TimevalEqual( when, d_lastUpdate ); } */ vrpn_bool old = !vrpn_TimevalGreater(when, d_lastUpdate); vrpn_bool equal = vrpn_TimevalEqual( when, d_lastUpdate ); // Is this "new" change idempotent? if ((d_mode & VRPN_SO_IGNORE_IDEMPOTENT) && (newValue == d_value)) { //fprintf(stderr, "... was idempotent.\n"); return vrpn_FALSE; } // Is this "new" change older than the previous change? if ( (d_mode & VRPN_SO_IGNORE_OLD) && old ) { //fprintf(stderr, "... was outdated.\n"); return vrpn_FALSE; } // Is this "new" change older than the previous change? if ((d_mode & VRPN_SO_IGNORE_OLD) && old) { // If the timestamps of the new & previous changes are equal: // - if we are the serializer, we can accept the local change // - if we are not the serializer, local updates are to be rejected if( equal ) { if( !d_isSerializer && isLocalSet ) { return vrpn_FALSE; } } else return vrpn_FALSE; } // All other clauses of shouldAcceptUpdate depend on serialization if (!(d_mode & VRPN_SO_DEFER_UPDATES)) { return vrpn_TRUE; } //fprintf(stderr, "... serializing: "); // If we're not the serializer, don't accept local set() calls - // forward those to the serializer. Non-local set() calls are // messages from the serializer that we should accept. if (!d_isSerializer) { if (isLocalSet) { //fprintf(stderr, "local update, not serializer, so reject.\n"); yankDeferredUpdateCallbacks(); return vrpn_FALSE; } else { //fprintf(stderr, "remote update, not serializer, so accept.\n"); return vrpn_TRUE; } } // We are the serializer. //fprintf(stderr, "serializer: "); if (isLocalSet) { //fprintf(stderr, "local update.\n"); if (d_policy == vrpn_DENY_LOCAL) { return vrpn_FALSE; } else { return vrpn_TRUE; } } // Are we accepting all updates? if (d_policy == vrpn_ACCEPT) { //fprintf(stderr, "policy is to accept.\n"); return vrpn_TRUE; } // Does the user want to accept this one? if ((d_policy == vrpn_CALLBACK) && d_policyCallback && (*d_policyCallback)(d_policyUserdata, newValue, when, this)) { //fprintf(stderr, "user callback accepts.\n"); return vrpn_TRUE; } //fprintf(stderr, "rejected.\n"); return vrpn_FALSE; }