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 }
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; }