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