Пример #1
0
void AttitudeFilterApp::getIMURawADCData() {
	int rc;
	
	{//begin of measurementMutex Lock
		Lock measurementLock(mMeasurementMutex);

		// Set state to measurement
		mCurrentState = measure;
		
		// Get current time to set timeout for measurement
		struct timeval now;
		struct timespec timeout;
			
		gettimeofday(&now, NULL);
		timeout = timeval_to_timespec(add_delta_us_to_timeval(now, mUSMeasurementTimeout));
		
		// Start waiting for measurement data
		rc = pthread_cond_timedwait(&mMeasurementCond, &mMeasurementMutex, &timeout);
		mCurrentState = doNothing;	// On error
		
		// Evaluate result of measurement
		if(rc == ETIMEDOUT) { // measurement timeout
			throw string("AttitudeFilterApp::getIMURawADCData: Measurement timed out!");
		} else if(rc != 0) { // pthread_cond_timewait error
			throw string("AttitudeFilterApp::getIMURawADCData: pthread_cond_timewait failed!");
		}
	}//end of measurementMutex Lock
}
Пример #2
0
struct timespec get_current_time(){
  struct timespec ts;
#if (defined _POSIX_TIMERS) && (_POSIX_TIMERS > 0)
  clock_gettime(CLOCK_REALTIME, &ts);
#else
  struct timeval tv;
  gettimeofday(&tv, NULL);
  ts =  timeval_to_timespec(tv);
#endif
  return ts;
}