struct timespec time_diff(struct timespec end, struct timespec start){ double difference = absTime(end)-absTime(start); struct timespec dT; dT.tv_sec = (int)difference; dT.tv_nsec = (difference-dT.tv_sec)*1000000000; return dT; }
void MyTimer::setTime(float t) { if (paused) timeValue = (long long)(t * 1000); else timeValue = absTime() - (long long)(t * 1000); }
void MyTimer::resume() { assert(paused && "Called resume() on a timer which was not paused!"); paused = false; timeValue = absTime() - timeValue; }
void MyTimer::pause() { assert(!paused && "Called pause() on a timer which was not running!"); paused = true; timeValue = absTime() - timeValue; }
static void main_run_ins(uint8_t data_valid) { struct timespec now; clock_gettime(TIMER, &now); double dt_imu_freq = 0.001953125; // 1/512; // doesn't work? ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq); if(MAG_READY(data_valid)){ ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise); } #if UPDATE_WITH_GRAVITY if(CLOSE_TO_GRAVITY(imu_float.accel)){ // use the gravity as reference ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3); } #endif if(GPS_READY(data_valid)){ ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise); } print_estimator_state(absTime(time_diff(now, start))); }
/* if 2 time values are close enough for X nanoseconds */ int is_Time_close(TimeInternal *x, TimeInternal *y, int nanos) { TimeInternal r1; TimeInternal r2; // first, subtract the 2 values. then call abs(), then call gtTime for requested the number of nanoseconds subTime(&r1, x, y); absTime(&r1); nano_to_Time(&r2, nanos); return !gtTime(&r1, &r2); }
void TL_ThreadManager::Destroy() { assert(pthread_equal(TL_Thread::MainThreatId(), pthread_self()) != 0); // neet to be sure all threads are gone TL_Timespec absTime(TL_Timespec::NOW); absTime << TL_Timespec::SECOND; ImplOf<TL_ThreadManager>::_Manager->cancelAllThreads( absTime ); delete ImplOf<TL_ThreadManager>::_Manager; ImplOf<TL_ThreadManager>::_Manager = NULL; }
//static void main_rawlog_dump(uint8_t data_valid) { static void main_rawlog_dump(struct AutopilotMessageVIUp* current_message) { struct timespec now; clock_gettime(TIMER, &now); struct raw_log_entry e; e.time = absTime(time_diff(now, start)); memcpy(&e.message, current_message, sizeof(*current_message)); write(raw_log_fd, &e, sizeof(e)); /*struct raw_log_entry e; e.time = absTime(time_diff(now, start)); e.message = current_message; RATES_COPY(e.gyro, imu_float.gyro); VECT3_COPY(e.accel, imu_float.accel); VECT3_COPY(e.mag, imu_float.mag); VECT3_COPY(e.ecef_pos, imu_ecef_pos); VECT3_COPY(e.ecef_vel, imu_ecef_vel); e.pressure_absolute = baro_measurement; e.data_valid = data_valid; write(raw_log_fd, &e, sizeof(e)); */ }
float MyTimer::getTime() { return (absTime() - timeValue) * (1.0f / 1000.0f); }
void MyTimer::start() { paused = false; timeValue = absTime(); }