int nix_rt_clock_settime(nix_clockid_t clockid, struct nix_timespec const *tp, nix_env_t *env) { #ifdef HAVE_CLOCK_SETTIME struct timespec nt; #else struct timeval nt; #endif if (tp == NULL) { nix_env_set_errno(env, EFAULT); return (-1); } nt.tv_sec = tp->tv_sec; #ifdef HAVE_CLOCK_SETTIME nt.tv_nsec = tp->tv_nsec; if (clock_settime(clockid, &nt) != 0) { nix_env_set_errno(env, errno); return (-1); } #else nt.tv_usec = tp->tv_nsec / 1000; if (clockid == NIX_CLOCK_REALTIME) { if (settimeofday(&nt, NULL) != 0) { nix_env_set_errno(env, errno); return (-1); } } else { nix_env_set_errno(env, EINVAL); return (-1); } #endif return (0); }
static int ClockTimeGetCurrent(epicsTimeStamp *pDest) { struct timespec clockNow; /* If a Hi-Res clock is available and works, use it */ #ifdef CLOCK_REALTIME_HR clock_gettime(CLOCK_REALTIME_HR, &clockNow) && #endif clock_gettime(CLOCK_REALTIME, &clockNow); if (!ClockTimePvt.synchronized && clockNow.tv_sec < POSIX_TIME_AT_EPICS_EPOCH) { clockNow.tv_sec = POSIX_TIME_AT_EPICS_EPOCH + 86400; clockNow.tv_nsec = 0; clock_settime(CLOCK_REALTIME, &clockNow); errlogPrintf("WARNING: OS Clock time was read before being set.\n" "Using 1990-01-02 00:00:00.000000 UTC\n"); } epicsTimeFromTimespec(pDest, &clockNow); return 0; }
int main(int argc, char *argv[]) { struct timespec tpset; tpset.tv_sec = TESTTIME; tpset.tv_nsec = 0; if (clock_settime(BOGUSCLOCKID, &tpset) == -1) { if (EINVAL == errno) { printf("Test PASSED\n"); return PTS_PASS; } else { printf("errno != EINVAL\n"); return PTS_FAIL; } } else { printf("clock_settime() did not return -1\n"); return PTS_UNRESOLVED; } printf("This test should not be executed.\n"); return PTS_UNRESOLVED; }
static void timesaver_restore(time_t secs_since_epoch) { if (secs_since_epoch) { struct timespec tp; tp.tv_sec = secs_since_epoch; tp.tv_nsec = 0; clock_settime(CLOCK_REALTIME, &tp); struct tm time; gmtime_r(&tp.tv_sec, &time); POWERDLOG(LOG_INFO, "%s Setting the time to be " "%02d-%02d-%04d %02d:%02d:%02d", __FUNCTION__, time.tm_mon+1, time.tm_mday, time.tm_year+1900, time.tm_hour, time.tm_min, time.tm_sec); } }
int main(void) { #ifdef CLOCK_MONOTONIC struct timespec tpset; tpset.tv_sec = TESTTIME; tpset.tv_nsec = 0; if (clock_settime(CLOCK_MONOTONIC, &tpset) == -1) { printf("Test PASSED\n"); return PTS_PASS; } else { printf("clock_settime() did not fail with CLOCK_MONOTONIC\n"); return PTS_FAIL; } return PTS_UNRESOLVED; #else printf("CLOCK_MONOTONIC not supported\n"); return PTS_UNSUPPORTED; #endif }
int cable_parallel_init() { int mode = IEEE1284_MODE_COMPAT; fd = open(devsys, O_RDWR | O_NONBLOCK); if (fd == -1) { perror("Unable to open the device desriptor\n"); fprintf(stderr, "Check permission of %s (eg. 'ls -la %s').\n", devsys, devsys); fprintf(stderr, "Your user ID can be added to %s's group to allow for unprivileged access.\n", devsys); return APP_ERR_INIT_FAILED; } // I don't know if these ioctl() are supported under FreeBSD #ifdef __LINUX_HOST__ if (ioctl(fd, PPCLAIM) == -1) { perror("Fail to claim the parallel port device interface.\n"); return APP_ERR_INIT_FAILED; } if (ioctl(fd, PPSETMODE, &mode) == -1) { perror("Setting compatibility mode on parallel port device failed.\n"); return APP_ERR_INIT_FAILED; } #endif #ifdef __PARALLEL_TIMER_BUSY_WAIT__ #ifdef PARALLEL_USE_PROCESS_TIMER struct timespec ts; ts.tv_sec = 0; ts.tv_nsec = 0; clock_settime(CLOCK_PROCESS_CPUTIME_ID, &ts); #else gettimeofday(&last_tv, NULL); #endif #endif return APP_ERR_NONE; }
kt_step(struct ocx *ocx, double offset) { double d; struct timespec ts; Put(ocx, OCX_TRACE, "KERNTIME_STEP %.3e\n", offset); d = floor(offset); offset -= d; AZ(clock_gettime(CLOCK_REALTIME, &ts)); ts.tv_sec += (long)d; ts.tv_nsec += (long)floor(offset * 1e9); if (ts.tv_nsec < 0) { ts.tv_sec -= 1; ts.tv_nsec += 1000000000; } else if (ts.tv_nsec >= 1000000000) { ts.tv_sec += 1; ts.tv_nsec -= 1000000000; } AZ(clock_settime(CLOCK_REALTIME, &ts)); TB_generation++; }
int main(int argc, char *argv[]) { struct timespec tpset; int i; int failure = 0; tpset.tv_sec = TESTTIME; tpset.tv_nsec = 0; if (geteuid() != 0) { printf("Test must be run as superuser\n"); return PTS_UNRESOLVED; } for (i = 0; i < NUMINVALIDTESTS; i++) { if (clock_settime(invalid_tests[i], &tpset) == -1) { if (EINVAL != errno) { printf("errno != EINVAL\n"); failure = 1; } } else { printf("clock_settime() did not return -1\n"); failure = 1; } } if (failure) { printf("At least one test FAILED -- see above\n"); return PTS_FAIL; } else { printf("All tests PASSED\n"); return PTS_PASS; } printf("This code should not be executed.\n"); return PTS_UNRESOLVED; }
int timesync_now_timespec(struct timespec *out) { int res; int gettime_ret; struct timespec now; #if (TIMESYNC_METHOD == TIMESYNC_METHOD_RFC868) gettime_ret = rfc868_gettime(&now); #elif (TIMESYNC_METHOD == TIMESYNC_METHOD_SNTP) gettime_ret = sntp_gettime(&now); #endif if (gettime_ret == 0) { int clk_ret; clk_ret = clock_settime(CLOCK_REALTIME, &now); if (clk_ret == 0) { if (out != NULL) { *out = now; } timespec_add(&now, &sync_interval, &next_sync); res = 1; } else { res = -1; } } else { res = -1; } return res; }
int cable_parallel_init() { if (ioperm(base, 3, 1)) { fprintf(stderr, "Couldn't get the port at %x\n", base); perror("Root privileges are required.\n"); return APP_ERR_INIT_FAILED; } printf("Connected to parallel port at %x\n", base); printf("Dropping root privileges.\n"); setreuid(getuid(), getuid()); #ifdef __PARALLEL_TIMER_BUSY_WAIT__ #ifdef PARALLEL_USE_PROCESS_TIMER struct timespec ts; ts.tv_sec = 0; ts.tv_nsec = 0; clock_settime(CLOCK_PROCESS_CPUTIME_ID, &ts); #else gettimeofday(&last_tv, NULL); #endif #endif return APP_ERR_NONE; }
static void ClockTimeSync(void *dummy) { taskwdInsert(0, NULL, NULL); for (epicsEventWaitWithTimeout(ClockTimePvt.loopEvent, ClockTimeSyncInterval); ClockTimePvt.synchronize == CLOCKTIME_SYNC; epicsEventWaitWithTimeout(ClockTimePvt.loopEvent, ClockTimeSyncInterval)) { epicsTimeStamp timeNow; int priority; if (generalTimeGetExceptPriority(&timeNow, &priority, LAST_RESORT_PRIORITY) == epicsTimeOK) { struct timespec clockNow; epicsTimeToTimespec(&clockNow, &timeNow); if (clock_settime(CLOCK_REALTIME, &clockNow)) { errlogPrintf("ClockTimeSync: clock_settime failed\n"); continue; } epicsMutexMustLock(ClockTimePvt.lock); if (!ClockTimePvt.synchronized) { ClockTimePvt.startTime = timeNow; ClockTimePvt.synchronized = 1; } ClockTimePvt.syncFromPriority = priority; ClockTimePvt.syncTime = timeNow; epicsMutexUnlock(ClockTimePvt.lock); } } ClockTimePvt.synchronized = 0; taskwdRemove(0); }
static Boolean setTime (ClockDriver *self, TimeInternal *time, Boolean force) { GET_CONFIG_CLOCKDRIVER(self, myConfig, unix); TimeInternal oldTime, delta; getTime(self, &oldTime); subTime(&delta, &oldTime, time); if(self->config.readOnly) { return FALSE; } if(force) { self->lockedUp = FALSE; } if(!force && !self->config.negativeStep && isTimeNegative(&delta)) { CRITICAL(THIS_COMPONENT"Cannot step Unix clock %s backwards\n", self->name); CRITICAL(THIS_COMPONENT"Manual intervention required or SIGUSR1 to force %s clock step\n", self->name); self->lockedUp = TRUE; self->setState(self, CS_NEGSTEP); return FALSE; } #if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) struct timespec tp; tp.tv_sec = time->seconds; tp.tv_nsec = time->nanoseconds; if(tp.tv_sec == 0) { ERROR(THIS_COMPONENT"Unix clock driver %s: cannot set time to zero seconds\n", self->name); return FALSE; } if(tp.tv_sec <= 0) { ERROR(THIS_COMPONENT"Unic clock driver %s: cannot set time to a negative value %d\n", self->name, tp.tv_sec); return FALSE; } #else struct timeval tv; tv.tv_sec = time->seconds; tv.tv_usec = time->nanoseconds / 1000; if(tv.tv_sec == 0) { ERROR(THIS_COMPONENT"Unix clock %s: cannot set time to zero seconds\n", self->name); return FALSE; } if(tv.tv_sec < 0) { ERROR(THIS_COMPONENT"Unic clock %s: cannot set time to a negative value %d\n", self->name, tv.tv_sec); return FALSE; } #endif /* _POSIX_TIMERS */ #if defined(_POSIX_TIMERS) && (_POSIX_TIMERS > 0) if (clock_settime(CLOCK_REALTIME, &tp) < 0) { PERROR(THIS_COMPONENT"Could not set system time"); return FALSE; } addTime(&_stepAccumulator, &_stepAccumulator, &delta); #else settimeofday(&tv, 0); addTime(&_stepAccumulator, &_stepAccumulator, &delta); #endif /* _POSIX_TIMERS */ if(oldTime.seconds != time->seconds) { updateXtmp_unix(oldTime, *time); if(myConfig->setRtc) { setRtc(self, time); } } self->_stepped = TRUE; struct timespec tmpTs = { time->seconds,0 }; char timeStr[MAXTIMESTR]; strftime(timeStr, MAXTIMESTR, "%x %X", localtime(&tmpTs.tv_sec)); NOTICE(THIS_COMPONENT"Unix clock %s: stepped the system clock to: %s.%d\n", self->name, timeStr, time->nanoseconds); self->setState(self, CS_FREERUN); return TRUE; }
int main(int argc, char *argv[]) { struct sigevent ev; struct sigaction act; struct timespec tpT0, tpclock, tsreset; struct itimerspec its; timer_t tid; int i, flags = 0, nocaught = 0; /* * set up sigevent for timer * set up sigaction to catch signal */ ev.sigev_notify = SIGEV_SIGNAL; ev.sigev_signo = SIGTOTEST; act.sa_handler=handler; act.sa_flags=0; if (sigemptyset(&act.sa_mask) != 0) { perror("sigemptyset() was not successful\n"); return PTS_UNRESOLVED; } if (sigaction(SIGTOTEST, &act, 0) != 0) { perror("sigaction() was not successful\n"); return PTS_UNRESOLVED; } if (clock_gettime(CLOCK_REALTIME, &tpT0) != 0) { perror("clock_gettime() was not successful\n"); return PTS_UNRESOLVED; } if (timer_create(CLOCK_REALTIME, &ev, &tid) != 0) { perror("timer_create() did not return success\n"); return PTS_UNRESOLVED; } flags |= TIMER_ABSTIME; its.it_interval.tv_sec = TIMERINTERVAL; its.it_interval.tv_nsec = 0; its.it_value.tv_sec = tpT0.tv_sec + TIMERINTERVAL; its.it_value.tv_nsec = tpT0.tv_nsec; if (timer_settime(tid, flags, &its, NULL) != 0) { perror("timer_settime() did not return success\n"); return PTS_UNRESOLVED; } sleep(TIMERINTERVAL); for (i = 0; i < ADDITIONALEXPIRES; i++) { sleep(TIMERINTERVAL); } tpclock.tv_sec = tpT0.tv_sec; tpclock.tv_nsec = tpT0.tv_nsec; getBeforeTime(&tsreset); if (clock_settime(CLOCK_REALTIME, &tpclock) != 0) { printf("clock_settime() was not successful\n"); return PTS_UNRESOLVED; } caught = 0; sleep(TIMERINTERVAL+ADDITIONALDELTA); if (caught == 1) { printf("Caught the first signal\n"); } else { printf("FAIL: Didn't catch timer after TIMERINTERVAL.\n"); nocaught = 1; } sleep(TIMERINTERVAL+ADDITIONALDELTA); if (caught >= 2) { printf("Caught another signal\n"); } else { printf("Caught %d < 2 signals\n", caught); nocaught = 1; } if (nocaught) { printf("Implementation does not repeat signals on clock reset\n"); } else { printf("Implementation does repeat signals on clock reset\n"); } // If we finish, pass tsreset.tv_sec += 2*(TIMERINTERVAL+ADDITIONALDELTA); setBackTime(tsreset); return PTS_PASS; }
void main(void) { uint8_t eth_init = ETH_INIT_DEFAULT; uint32_t server; struct timespec time; strncpy(_tz.tzname, TIMEZONE_CODE, sizeof(_tz.tzname) - 1); _tz.timezone = TIMEZONE_SECS; if (doesclrscrafterexit()) { atexit(confirm_exit); } #ifdef __APPLE2__ { int file; printf("\nSetting slot "); file = open("ethernet.slot", O_RDONLY); if (file != -1) { read(file, ð_init, 1); close(file); eth_init &= ~'0'; } printf("- %d\n", eth_init); } #endif printf("\nInitializing "); if (ip65_init(eth_init)) { error_exit(); } printf("- Ok\n\nObtaining IP address "); if (dhcp_init()) { error_exit(); } printf("- Ok\n\nResolving %s ", NTP_SERVER); server = dns_resolve(NTP_SERVER); if (!server) { error_exit(); } printf("- Ok\n\nGetting %s ", _tz.tzname); time.tv_sec = sntp_get_time(server); if (!time.tv_sec) { error_exit(); } // Convert time from seconds since 1900 to // seconds since 1970 according to RFC 868 time.tv_sec -= 2208988800UL; printf("- %s", ctime(&time.tv_sec)); time.tv_nsec = 0; clock_settime(CLOCK_REALTIME, &time); }
int main(int argc, char *argv[]) { struct timespec tsT0, tsT1; int pid; /* Check that we're root...can't call clock_settime with CLOCK_REALTIME otherwise */ if (getuid() != 0) { printf("Run this test as ROOT, not as a Regular User\n"); return PTS_UNTESTED; } if (clock_gettime(CLOCK_REALTIME, &tsT0) != 0) { perror("clock_gettime() did not return success\n"); return PTS_UNRESOLVED; } if ((pid = fork()) == 0) { /* child here */ int flags = 0; struct timespec tsT2; tsT1.tv_sec = tsT0.tv_sec + SLEEPOFFSET; tsT1.tv_nsec = tsT0.tv_nsec; flags |= TIMER_ABSTIME; if (clock_nanosleep(CLOCK_REALTIME, flags, &tsT1, NULL) != 0) { printf("clock_nanosleep() did not return success\n"); return CHILDFAIL; } if (clock_gettime(CLOCK_REALTIME, &tsT2) != 0) { perror("clock_gettime() did not return success\n"); return CHILDFAIL; } if (tsT2.tv_sec >= tsT1.tv_sec) { if ((tsT2.tv_sec - tsT1.tv_sec) <= ACCEPTABLEDELTA) { return CHILDPASS; } else { printf("Ended too late. %d >> %d\n", (int)tsT2.tv_sec, (int)tsT1.tv_sec); return CHILDFAIL; } } else { printf("Did not sleep for long enough %d < %d\n", (int)tsT2.tv_sec, (int)tsT1.tv_sec); return CHILDFAIL; } return CHILDFAIL; } else { /* parent here */ int i; struct timespec tsreset; sleep(SMALLTIME); if (clock_settime(CLOCK_REALTIME, &tsT0) != 0) { printf("clock_settime() did not return success\n"); return PTS_UNRESOLVED; } if (wait(&i) == -1) { perror("Error waiting for child to exit\n"); return PTS_UNRESOLVED; } getBeforeTime(&tsreset); // get current time tsreset.tv_sec += SMALLTIME; setBackTime(tsreset); if (WIFEXITED(i) && WEXITSTATUS(i)) { printf("Test PASSED\n"); return PTS_PASS; } else { printf("Test FAILED\n"); return PTS_FAIL; } } return PTS_UNRESOLVED; }
int do_cs_test(void) { int ret; int result=0; pthread_mutex_t mtxN, mtxI; pthread_cond_t cndN; pthread_condattr_t ca; pthread_t thN, thD; struct timespec ts, timeout; clockid_t cid; /* The 3 next data aim to minimize the impact on the * system time, when the monotonic clock is supported */ long monotonic_clk; struct timespec diff; char sens=0; /* We are going to initialize the cond vars and the mutexes */ dtN.pmtx = &mtxN; dtI.pmtx = &mtxI; dtN.pcnd = &cndN; dtI.pcnd = &cndI; ret = pthread_mutex_init(&mtxI, NULL); if (ret != 0) { UNRESOLVED(ret, "Unable to initialize a default mutex"); } ret = pthread_mutex_init(&mtxN, NULL); if (ret != 0) { UNRESOLVED(ret, "Unable to initialize a default mutex"); } ret = pthread_condattr_init(&ca); if (ret != 0) { UNRESOLVED(ret, "Unable to initialize cond attribute object"); } #if 0 /* Test if the testcase is valid: change the clock from the "NULL" cond var */ pthread_condattr_setclock(&ca, CLOCK_MONOTONIC); ret = pthread_cond_init(&cndN, &ca); pthread_condattr_setclock(&ca, CLOCK_REALTIME); #else ret = pthread_cond_init(&cndN, NULL); #endif if (ret != 0) { UNRESOLVED(ret, "Unable to initialize the NULL attribute conditional variable."); } dtI.ctrl = 0; dtN.ctrl = 0; dtI.rc = 0; dtN.rc = 0; #if VERBOSE > 1 output("Data initialized successfully for CS test.\n"); #endif monotonic_clk = sysconf(_SC_MONOTONIC_CLOCK); #if VERBOSE > 1 output("Sysconf for monotonix clock: %li\n", monotonic_clk); #endif /* Get the default clock ID */ ret = pthread_condattr_getclock(&ca, &cid); if (ret != 0) { UNRESOLVED(ret, "Unable to get clockid from the default cond attribute object"); } /* Check whether we can set the system clock */ /* Backup the current monotonic time if available */ if (monotonic_clk != -1L) { ret = clock_gettime(CLOCK_MONOTONIC, &diff); if (ret != 0) { output("Clock_gettime(CLOCK_MONOTONIC) failed when the system claims to support this option\n"); monotonic_clk = -1L; } } ret = clock_gettime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to get clock time"); } ret = clock_settime(cid, &ts); if (ret != 0) { #if VERBOSE > 1 output("clock_settime failed (%s)\n", strerror(errno)); output("We cannot test if both cond uses the same clock then...\n"); #endif UNTESTED("Was unable to set the default clock time. Need more privileges?"); } else /* We can do the test */ { #if VERBOSE > 1 output("clock_settime succeeded\n"); #endif if (monotonic_clk != -1L) { #if VERBOSE > 2 output("Monotonic clock : %10i.%09li\n", diff.tv_sec, diff.tv_nsec); output("Default clock : %10i.%09li\n", ts.tv_sec, ts.tv_nsec); #endif /* Save the decay between default clock and clock MONOTONIC */ if (diff.tv_sec > ts.tv_sec) { sens = -1; /* monotonic was bigger than system */ if (diff.tv_nsec < ts.tv_nsec) { diff.tv_nsec += 1000000000 - ts.tv_nsec; diff.tv_sec -= 1 + ts.tv_sec; } else { diff.tv_nsec -= ts.tv_nsec; diff.tv_sec -= ts.tv_sec; } } else { if (diff.tv_sec == ts.tv_sec) { diff.tv_sec = 0; if (diff.tv_nsec > ts.tv_nsec) { sens = -1; diff.tv_nsec -= ts.tv_nsec; } else { sens = 1; /* Default clock was bigger than monotonic */ diff.tv_nsec = ts.tv_nsec - diff.tv_nsec; } } else /* ts.tv_sec > diff.tv_sec */ { sens = 1; if (ts.tv_nsec < diff.tv_nsec) { diff.tv_nsec = 1000000000 + ts.tv_nsec - diff.tv_nsec; diff.tv_sec = ts.tv_sec - (1 + diff.tv_sec); } else { diff.tv_nsec = ts.tv_nsec - diff.tv_nsec; diff.tv_sec = ts.tv_sec - diff.tv_sec; } } } #if VERBOSE > 2 output("Computed diff : %10i.%09li\n", diff.tv_sec, diff.tv_nsec); output("With monotonic %s than default\n", sens>0?"smaller":"bigger"); #endif } /* Prepare the timeout parameters */ timeout.tv_nsec = 0; timeout.tv_sec = ts.tv_sec + 260000; /* About 3 days later */ dtN.timeout = &timeout; dtI.timeout = &timeout; /* create the threads */ ret = pthread_create(&thD, NULL, test_timeout, &dtI); if (ret != 0) { UNRESOLVED(ret, "Unable to create a thread"); } ret = pthread_create(&thN, NULL, test_timeout, &dtN); if (ret != 0) { UNRESOLVED(ret, "Unable to create a thread"); } /* Lock the two mutex and make sure the threads are in wait */ ret = pthread_mutex_lock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } while ((dtN.ctrl & FLAG_INWAIT) == 0) { ret = pthread_mutex_unlock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } sched_yield(); ret = pthread_mutex_lock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } } ret = pthread_mutex_lock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } while ((dtI.ctrl & FLAG_INWAIT) == 0) { ret = pthread_mutex_unlock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } sched_yield(); ret = pthread_mutex_lock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } } /* Now both threads are in the timed wait */ #if VERBOSE > 1 output("Two threads are created and waiting.\n"); output("About to change the default clock value.\n"); #endif /* We re-read the default clock time, to introduce minimal error on the clock */ ret = clock_gettime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to get clock time"); } ts.tv_sec += 604800; /* Exactly 1 week forth */ /* We set the clock to a date after the timeout parameter */ ret = clock_settime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to set clock time (again)"); } /* unlock the two mutex */ ret = pthread_mutex_unlock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } ret = pthread_mutex_unlock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } /* Let the others threads run */ sched_yield(); #if VERBOSE > 1 output("Checking that both threads have timedout...\n"); #endif /* Relock the mutexs */ ret = pthread_mutex_lock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } ret = pthread_mutex_lock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } /* Check whether the thread has timedout */ if (dtI.rc == 0) { #if VERBOSE > 0 output("The thread was not woken when the clock was changed so as the timeout expired\n"); output("Going to simulate this POSIX behavior...\n"); #endif ret = pthread_cond_signal(&cndN); if (ret != 0) { UNRESOLVED(ret, "Unable to signal the Null attribute condition"); } ret = pthread_cond_signal(&cndI); if (ret != 0) { UNRESOLVED(ret, "Unable to signal the Default attribute condition"); } /* The threads will now report a spurious wake up ... */ /* We give the threads another chance to timeout */ /* unlock the two mutex */ ret = pthread_mutex_unlock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } ret = pthread_mutex_unlock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } /* Let the others threads run */ sched_yield(); #if VERBOSE > 1 output("Rechecking that both threads have timedout...\n"); #endif /* Relock the mutexs */ ret = pthread_mutex_lock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } ret = pthread_mutex_lock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to lock a mutex"); } } /* Process the Null condvar */ if ((dtN.ctrl & FLAG_TIMEDOUT) != 0) { #if VERBOSE > 1 output("Null attribute cond var timed out\n"); #endif /* Join the thread */ ret = pthread_join(thN, NULL); if (ret != 0) { UNRESOLVED(ret, "Join thread failed"); } /* Unlock the mutex */ ret = pthread_mutex_unlock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } } else { #if VERBOSE > 1 output("Null attribute cond var did not time out\n"); #endif /* Signal the condition */ dtN.ctrl |= FLAG_CONDWAKE; ret = pthread_cond_signal(&cndN); if (ret != 0) { UNRESOLVED(ret, "Unable to signal the Null attribute condition"); } /* Unlock the mutex and join the thread */ ret = pthread_mutex_unlock(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } /* Join the thread */ ret = pthread_join(thN, NULL); if (ret != 0) { UNRESOLVED(ret, "Join thread failed"); } } /* Process the Default condvar */ if ((dtI.ctrl & FLAG_TIMEDOUT) != 0) { #if VERBOSE > 1 output("Default attribute cond var timed out\n"); #endif /* Join the thread */ ret = pthread_join(thD, NULL); if (ret != 0) { UNRESOLVED(ret, "Join thread failed"); } /* Unlock the mutex */ ret = pthread_mutex_unlock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } } else { #if VERBOSE > 1 output("Default attribute cond var did not time out\n"); #endif /* Signal the condition */ dtI.ctrl |= FLAG_CONDWAKE; ret = pthread_cond_signal(&cndI); if (ret != 0) { UNRESOLVED(ret, "Unable to signal the Default attribute condition"); } /* Unlock the mutex and join the thread */ ret = pthread_mutex_unlock(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Unable to unlock a mutex"); } /* Join the thread */ ret = pthread_join(thD, NULL); if (ret != 0) { UNRESOLVED(ret, "Join thread failed"); } } /* Set the system time back to its value */ if (monotonic_clk == -1L) /* Monotonic is not supported */ { ret = clock_gettime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to get clock time"); } ts.tv_sec -= 604800; /* Exactly 1 week back */ /* We set the clock to a date after the timeout parameter */ ret = clock_settime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to set clock time (3rd time)"); } #if VERBOSE > 1 output("Default clock was set back\n"); #endif } else { /* Read the monotonic time */ ret = clock_gettime(CLOCK_MONOTONIC, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to get monotonic clock time"); } /* Apply the difference back */ if (sens > 0) /* monotonic was smaller than system => we must add the diff value */ { ts.tv_sec += diff.tv_sec; ts.tv_nsec += diff.tv_nsec; if (ts.tv_nsec >= 1000000000) { ts.tv_nsec -= 1000000000; ts.tv_sec += 1; } } else /* monotonic was bigger than system => we must remove the diff value */ { ts.tv_sec -= diff.tv_sec; if (ts.tv_nsec < diff.tv_nsec) { ts.tv_sec -= 1; ts.tv_nsec += 1000000000 - diff.tv_nsec; } else { ts.tv_nsec -= diff.tv_nsec; } } /* We set the clock to a date after the timeout parameter */ ret = clock_settime(cid, &ts); if (ret != 0) { UNRESOLVED(errno, "Unable to set clock time (3rd time)"); } #if VERBOSE > 1 output("Default clock was set back using monotonic clock as a reference\n"); #endif } /* Compare the two cond vars */ #if VERBOSE > 2 output("Default attribute cond var timedwait return value: %d\n", dtI.rc); output("Default attribute cond var exited with a timeout : %s\n", (dtI.ctrl & FLAG_TIMEDOUT)?"yes":"no"); output("Default attribute cond var had to be signaled : %s\n", (dtI.ctrl & FLAG_CONDWAKE)?"yes":"no"); output("Default attribute cond var exited as signaled : %s\n", (dtI.ctrl & FLAG_AWAKEN)?"yes":"no"); output("Default attribute cond var spurious wakeups : %d\n", (dtI.ctrl & MASK_COUNTER) - 1); output(" Null attribute cond var timedwait return value: %d\n", dtN.rc); output(" Null attribute cond var exited with a timeout : %s\n", (dtN.ctrl & FLAG_TIMEDOUT)?"yes":"no"); output(" Null attribute cond var had to be signaled : %s\n", (dtN.ctrl & FLAG_CONDWAKE)?"yes":"no"); output(" Null attribute cond var exited as signaled : %s\n", (dtN.ctrl & FLAG_AWAKEN)?"yes":"no"); output(" Null attribute cond var spurious wakeups : %d\n", (dtN.ctrl & MASK_COUNTER) - 1); #endif if ((dtN.ctrl == dtI.ctrl) && (dtN.rc == dtI.rc)) { result = 0; #if VERBOSE > 1 output("The two cond vars behaved exactly in the same maneer\n"); #endif } else { if ((dtN.ctrl & FLAG_TIMEDOUT) != (dtI.ctrl & FLAG_TIMEDOUT)) { result += 1; /* The test has failed */ } else { if (dtN.rc != dtI.rc) { output("Error codes were different: N:%d D:%d\n",dtN.rc, dtI.rc); UNRESOLVED(dtN.rc>dtI.rc?dtN.rc:dtI.rc, "Different error codes?"); } if ((dtN.ctrl & FLAG_AWAKEN) == (dtI.ctrl & FLAG_AWAKEN)) { /* The number of spurious wakeups is different */ output("Different spurious wakeups: N:%d D:%d\n", (dtN.ctrl & MASK_COUNTER) - 1, (dtI.ctrl & MASK_COUNTER) - 1); result = 0; /* We don't consider this as a fail case */ } } } } /* We can cleanup things now */ ret = pthread_cond_destroy(&cndN); if (ret != 0) { UNRESOLVED(ret, "Cond destroy failed"); } ret = pthread_condattr_destroy(&ca); if (ret != 0) { UNRESOLVED(ret, "Cond attr destroy failed"); } ret = pthread_mutex_destroy(&mtxN); if (ret != 0) { UNRESOLVED(ret, "Mutex destroy failed"); } ret = pthread_mutex_destroy(&mtxI); if (ret != 0) { UNRESOLVED(ret, "Mutex destroy failed"); } return result; }
void MTK::handle_message(gps_mtk_packet_t &packet) { if (_mtk_revision == 16) { _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 } else if (_mtk_revision == 19) { _gps_position->lat = packet.latitude; // both degrees*1e7 _gps_position->lon = packet.longitude; // both degrees*1e7 } else { warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; // Indicate this data is not usable and bail out _gps_position->eph = 1000.0f; _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; _gps_position->eph = packet.hdop / 100.0f; // from cm to m _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; uint32_t timeinfo_conversion_temp; timeinfo.tm_mday = packet.date / 10000; timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000; timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1; timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100; timeinfo.tm_hour = (packet.utc_time / 10000000); timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000; timeinfo.tm_min = timeinfo_conversion_temp / 100000; timeinfo_conversion_temp -= timeinfo.tm_min * 100000; timeinfo.tm_sec = timeinfo_conversion_temp / 1000; timeinfo_conversion_temp -= timeinfo.tm_sec * 1000; time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // and control its drift. Since we rely on the HRT for our monotonic // clock, updating it from time to time is safe. timespec ts; ts.tv_sec = epoch; ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; } else { _gps_position->time_utc_usec = 0; } _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time _rate_count_vel++; _rate_count_lat_lon++; return; }
int main(void) { struct timespec t1; clock_settime(CLOCK_REALTIME, &t1); return 0; }
rtems_task Init( rtems_task_argument argument ) { struct timespec tv; struct timespec tr; int sc; time_t seconds; time_t seconds1; unsigned int remaining; struct tm tm; struct timespec delay_request; puts( "\n\n*** POSIX CLOCK TEST ***" ); tm_build_time( &tm, TM_FRIDAY, TM_MAY, 24, 96, 11, 5, 0 ); /* error cases in clock_gettime and clock_settime */ puts( "Init: clock_gettime - EINVAL (NULL timespec)" ); sc = clock_gettime( CLOCK_REALTIME, NULL ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); puts( "Init: clock_gettime - EINVAL (invalid clockid)" ); sc = clock_gettime( (clockid_t)-1, &tv ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); puts( "Init: clock_settime - EINVAL (invalid clockid)" ); sc = clock_settime( (clockid_t)-1, &tv ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); /* way back near the dawn of time :D */ tv.tv_sec = 1; tv.tv_nsec = 0; printf( ctime( &tv.tv_sec ) ); puts( "Init: clock_settime - before 1988 EINVAL" ); sc = clock_settime( CLOCK_REALTIME, &tv ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); /* exercise clock_getres */ puts( "Init: clock_getres - EINVAL (invalid clockid)" ); sc = clock_getres( (clockid_t) -1, &tv ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); puts( "Init: clock_getres - EINVAL (NULL resolution)" ); sc = clock_getres( CLOCK_REALTIME, NULL ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); puts( "Init: clock_getres - SUCCESSFUL" ); sc = clock_getres( CLOCK_REALTIME, &tv ); printf( "Init: resolution = sec (%" PRIdtime_t "), nsec (%ld)\n", tv.tv_sec, tv.tv_nsec ); rtems_test_assert( !sc ); /* set the time of day, and print our buffer in multiple ways */ tv.tv_sec = mktime( &tm ); rtems_test_assert( tv.tv_sec != -1 ); tv.tv_nsec = 0; /* now set the time of day */ empty_line(); printf( asctime( &tm ) ); puts( "Init: clock_settime - SUCCESSFUL" ); sc = clock_settime( CLOCK_REALTIME, &tv ); rtems_test_assert( !sc ); printf( asctime( &tm ) ); printf( ctime( &tv.tv_sec ) ); /* use sleep to delay */ remaining = sleep( 3 ); rtems_test_assert( !remaining ); /* print new times to make sure it has changed and we can get the realtime */ sc = clock_gettime( CLOCK_PROCESS_CPUTIME_ID, &tv ); rtems_test_assert( !sc ); printf("Time since boot: (%" PRIdtime_t ", %ld)\n", tv.tv_sec,tv.tv_nsec ); sc = clock_gettime( CLOCK_REALTIME, &tv ); rtems_test_assert( !sc ); printf( ctime( &tv.tv_sec ) ); seconds = time( NULL ); printf( ctime( &seconds ) ); /* just to have the value copied out through the parameter */ seconds = time( &seconds1 ); rtems_test_assert( seconds == seconds1 ); /* check the time remaining */ printf( "Init: seconds remaining (%d)\n", (int)remaining ); rtems_test_assert( !remaining ); /* error cases in nanosleep */ empty_line(); puts( "Init: nanosleep - EINVAL (NULL time)" ); sc = nanosleep ( NULL, &tr ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); tv.tv_sec = 0; tv.tv_nsec = TOD_NANOSECONDS_PER_SECOND * 2; puts( "Init: nanosleep - EINVAL (too many nanoseconds)" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); /* this is an error */ tv.tv_sec = -1; tv.tv_nsec = 0; puts( "Init: nanosleep - negative seconds - EINVAL" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); /* this is also an error */ tv.tv_sec = 0; tv.tv_nsec = -1; puts( "Init: nanosleep - negative nanoseconds - EINVAL" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( sc == -1 ); rtems_test_assert( errno == EINVAL ); /* this is actually a small delay */ tv.tv_sec = 0; tv.tv_nsec = 1; puts( "Init: nanosleep - delay so small results in one tick" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( !sc ); rtems_test_assert( !tr.tv_sec ); rtems_test_assert( !tr.tv_nsec ); /* use nanosleep to yield */ tv.tv_sec = 0; tv.tv_nsec = 0; puts( "Init: nanosleep - yield with remaining" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( !sc ); rtems_test_assert( !tr.tv_sec ); rtems_test_assert( !tr.tv_nsec ); puts( "Init: nanosleep - yield with NULL time remaining" ); sc = nanosleep ( &tv, NULL ); rtems_test_assert( !sc ); rtems_test_assert( !tr.tv_sec ); rtems_test_assert( !tr.tv_nsec ); /* use nanosleep to delay */ tv.tv_sec = 3; tv.tv_nsec = 500000; puts( "Init: nanosleep - 1.05 seconds" ); sc = nanosleep ( &tv, &tr ); rtems_test_assert( !sc ); /* print the current real time again */ sc = clock_gettime( CLOCK_REALTIME, &tv ); rtems_test_assert( !sc ); printf( ctime( &tv.tv_sec ) ); /* check the time remaining */ printf( "Init: sec (%" PRIdtime_t "), nsec (%ld) remaining\n", tr.tv_sec, tr.tv_nsec ); rtems_test_assert( !tr.tv_sec && !tr.tv_nsec ); puts( "Init: nanosleep - 1.35 seconds" ); delay_request.tv_sec = 1; delay_request.tv_nsec = 35000000; sc = nanosleep( &delay_request, NULL ); rtems_test_assert( !sc ); /* print the current real time again */ sc = clock_gettime( CLOCK_REALTIME, &tv ); rtems_test_assert( !sc ); printf( ctime( &tv.tv_sec ) ); empty_line(); puts( "clock_gettime - CLOCK_THREAD_CPUTIME_ID -- ENOSYS" ); #if defined(_POSIX_THREAD_CPUTIME) { struct timespec tp; sc = clock_gettime( CLOCK_THREAD_CPUTIME_ID, &tp ); check_enosys( sc ); } #endif puts( "clock_settime - CLOCK_PROCESS_CPUTIME_ID -- ENOSYS" ); #if defined(_POSIX_CPUTIME) { struct timespec tp; sc = clock_settime( CLOCK_PROCESS_CPUTIME_ID, &tp ); check_enosys( sc ); } #endif puts( "clock_settime - CLOCK_THREAD_CPUTIME_ID -- ENOSYS" ); #if defined(_POSIX_THREAD_CPUTIME) { struct timespec tp; sc = clock_settime( CLOCK_THREAD_CPUTIME_ID, &tp ); check_enosys( sc ); } #endif puts( "*** END OF POSIX CLOCK TEST ***" ); rtems_test_exit(0); }
/** * Finish payload rx */ int // 0 = no message handled, 1 = message handled, 2 = sat info message handled UBX::payload_rx_done(void) { int ret = 0; // return if no message handled if (_rx_state != UBX_RXMSG_HANDLE) { return ret; } // handle message switch (_rx_msg) { case UBX_MSG_NAV_PVT: UBX_TRACE_RXMSG("Rx NAV-PVT\n"); //Check if position fix flag is good if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; _gps_position->vel_ned_valid = true; } else { _gps_position->fix_type = 0; _gps_position->vel_ned_valid = false; } _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; _gps_position->lat = _buf.payload_rx_nav_pvt.lat; _gps_position->lon = _buf.payload_rx_nav_pvt.lon; _gps_position->alt = _buf.payload_rx_nav_pvt.hMSL; _gps_position->eph = (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f; _gps_position->epv = (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f; _gps_position->vel_m_s = (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f; _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; //Check if time and date fix flags are good if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; timeinfo.tm_mon = _buf.payload_rx_nav_pvt.month - 1; timeinfo.tm_mday = _buf.payload_rx_nav_pvt.day; timeinfo.tm_hour = _buf.payload_rx_nav_pvt.hour; timeinfo.tm_min = _buf.payload_rx_nav_pvt.min; timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); if (epoch > GPS_EPOCH_SECS) { // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // and control its drift. Since we rely on the HRT for our monotonic // clock, updating it from time to time is safe. timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } else { _gps_position->time_utc_usec = 0; } } _gps_position->timestamp_time = hrt_absolute_time(); _gps_position->timestamp_velocity = hrt_absolute_time(); _gps_position->timestamp_variance = hrt_absolute_time(); _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_vel++; _rate_count_lat_lon++; _got_posllh = true; _got_velned = true; ret = 1; break; case UBX_MSG_NAV_POSLLH: UBX_TRACE_RXMSG("Rx NAV-POSLLH\n"); _gps_position->lat = _buf.payload_rx_nav_posllh.lat; _gps_position->lon = _buf.payload_rx_nav_posllh.lon; _gps_position->alt = _buf.payload_rx_nav_posllh.hMSL; _gps_position->eph = (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m _gps_position->epv = (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m _gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height; _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_lat_lon++; _got_posllh = true; ret = 1; break; case UBX_MSG_NAV_SOL: UBX_TRACE_RXMSG("Rx NAV-SOL\n"); _gps_position->fix_type = _buf.payload_rx_nav_sol.gpsFix; _gps_position->s_variance_m_s = (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f; // from cm to m _gps_position->satellites_used = _buf.payload_rx_nav_sol.numSV; _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; break; case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; timeinfo.tm_mday = _buf.payload_rx_nav_timeutc.day; timeinfo.tm_hour = _buf.payload_rx_nav_timeutc.hour; timeinfo.tm_min = _buf.payload_rx_nav_timeutc.min; timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); // only set the time if it makes sense if (epoch > GPS_EPOCH_SECS) { // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // and control its drift. Since we rely on the HRT for our monotonic // clock, updating it from time to time is safe. timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } else { _gps_position->time_utc_usec = 0; } } _gps_position->timestamp_time = hrt_absolute_time(); ret = 1; break; case UBX_MSG_NAV_SVINFO: UBX_TRACE_RXMSG("Rx NAV-SVINFO\n"); // _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp _satellite_info->timestamp = hrt_absolute_time(); ret = 2; break; case UBX_MSG_NAV_VELNED: UBX_TRACE_RXMSG("Rx NAV-VELNED\n"); _gps_position->vel_m_s = (float)_buf.payload_rx_nav_velned.speed * 1e-2f; _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */ _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */ _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */ _gps_position->cog_rad = (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); _rate_count_vel++; _got_velned = true; ret = 1; break; case UBX_MSG_MON_VER: UBX_TRACE_RXMSG("Rx MON-VER\n"); ret = 1; break; case UBX_MSG_MON_HW: UBX_TRACE_RXMSG("Rx MON-HW\n"); switch (_rx_payload_length) { case sizeof(ubx_payload_rx_mon_hw_ubx6_t): /* u-blox 6 msg format */ _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx6.noisePerMS; _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx6.jamInd; ret = 1; break; case sizeof(ubx_payload_rx_mon_hw_ubx7_t): /* u-blox 7+ msg format */ _gps_position->noise_per_ms = _buf.payload_rx_mon_hw_ubx7.noisePerMS; _gps_position->jamming_indicator = _buf.payload_rx_mon_hw_ubx7.jamInd; ret = 1; break; default: // unexpected payload size: ret = 0; // don't handle message break; } break; case UBX_MSG_ACK_ACK: UBX_TRACE_RXMSG("Rx ACK-ACK\n"); if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_ACK; } ret = 1; break; case UBX_MSG_ACK_NAK: UBX_TRACE_RXMSG("Rx ACK-NAK\n"); if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) { _ack_state = UBX_ACK_GOT_NAK; } ret = 1; break; default: break; } return ret; }
int sif_main(int argc, char *argv[]) { if (argc >= 2) { if (!strcmp(argv[1], "init")) { return sif_init(); } else if (!strcmp(argv[1], "gpio") && argc == 4) { vsn_sif.gpio[0] = atoi(argv[2]); vsn_sif.gpio[1] = atoi(argv[3]); sif_gpio1_update(); sif_gpio2_update(); printf("GPIO States: %2x %2x\n", vsn_sif.gpio[0], vsn_sif.gpio[1]); return 0; } else if (!strcmp(argv[1], "pwr") && argc == 3) { int val = atoi(argv[2]); //STM32_TIM_SETCOMPARE(vsn_sif.tim8, GPIO_OUT_PWRPWM_TIM8_CH, val); STM32_TIM_SETCOMPARE(vsn_sif.tim3, GPIO_OUT_PWM_TIM3_CH, val); return 0; } else if (!strcmp(argv[1], "time") && argc == 3) { struct timespec t_set; t_set.tv_sec = atoi(argv[2]); clock_settime(CLOCK_REALTIME, &t_set); } else if (!strcmp(argv[1], "free")) { size_t page = 0, stpage = 0xFFFF; ssize_t status; do { status = up_progmem_ispageerased(page++); /* Is this beginning of new free space section */ if (status == 0) { if (stpage == 0xFFFF) stpage = page-1; } else if (status != 0) { if (stpage != 0xFFFF) { printf("Free Range:\t%lu\t-\t%lu\n", (unsigned long)stpage, (unsigned long)(page-2)); stpage = 0xFFFF; } } } while (status >= 0); return 0; } else if (!strcmp(argv[1], "erase") && argc == 3) { size_t page = atoi(argv[2]); printf("Erase result: %d\n", up_progmem_erasepage(page)); return 0; } else if (!strcmp(argv[1], "flash") && argc == 3) { size_t page = atoi(argv[2]); size_t addr = page * up_progmem_pagesize(page); printf("Write result: %d (writing to address %lxh)\n", up_progmem_write(addr, "Test", 4), (unsigned long)addr); return 0; } else if (!strcmp(argv[1], "i2c") && argc == 3) { int val = atoi(argv[2]); I2C_SETFREQUENCY(vsn_sif.i2c1, 100000); struct lis331dl_dev_s * lis = lis331dl_init(vsn_sif.i2c1, val); if (lis) { const struct lis331dl_vector_s * a; int i; uint32_t time_stamp = clock_systimer(); /* Set to 400 Hz : 3 = 133 Hz/axis */ lis331dl_setconversion(lis, false, true); /* Sample some values */ for (i=0; i<1000;) { if ((a = lis331dl_getreadings(lis))) { i++; printf("%d %d %d\n", a->x, a->y, a->z); } else if (errno != 11) { printf("Readings errno %d\n", errno); break; } } printf("Time diff = %d\n", clock_systimer() - time_stamp); lis331dl_deinit(lis); } else printf("Exit point: errno=%d\n", errno); return 0; } else if (!strcmp(argv[1], "pga")) { int gain = atoi(argv[2]); gain = vsn_muxbus_setpgagain(gain); printf("Gain changed: %d\n", gain); return 0; } else if (!strcmp(argv[1], "cc")) { struct cc1101_dev_s * cc; uint8_t buf[64]; int sta; cc = cc1101_init(vsn_sif.spi2, CC1101_PIN_GDO0, GPIO_CC1101_GDO0, &cc1101_rfsettings_ISM1_868MHzGFSK100kbps); if (cc) { /* Work-around: enable falling edge, event and interrupt */ stm32_gpiosetevent(GPIO_CC1101_GDO0, false, true, true, cc1101_eventcb); /* Enable clock to ARM PLL, allowing to speed-up to 72 MHz */ cc1101_setgdo(cc, CC1101_PIN_GDO2, CC1101_GDO_CLK_XOSC3); cc1101_setchannel(cc, 0); /* AV Test Hex, receive on that channel */ cc1101_receive(cc); /* Enter RX mode */ while (1) { fflush(stdout); sta = cc1101_read(cc, buf, 64); if (sta > 0) { printf("Received %d bytes: rssi=%d [dBm], LQI=%d (CRC %s)\n", sta, cc1101_calcRSSIdBm(buf[sta-2]), buf[sta-1]&0x7F, (buf[sta-1]&0x80)?"OK":"BAD"); cc1101_write(cc, buf, 61); cc1101_send(cc); printf("Packet send back\n"); cc1101_receive(cc); } } } } } fprintf(stderr, "%s:\tinit\n\tgpio\tA B\n\tpwr\tval\n", argv[0]); fprintf(stderr, "rtc time = %u, time / systick = %u / %u\n", up_rtc_time(), time(NULL), clock_systimer()); return -1; }
int UBX::handle_message() { int ret = 0; if (_configured) { /* handle only info messages when configured */ switch (_message_class) { case UBX_CLASS_NAV: switch (_message_id) { case UBX_MESSAGE_NAV_POSLLH: { // printf("GOT NAV_POSLLH\n"); gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; _gps_position->lat = packet->lat; _gps_position->lon = packet->lon; _gps_position->alt = packet->height_msl; _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_lat_lon++; _got_posllh = true; ret = 1; break; } case UBX_MESSAGE_NAV_SOL: { // printf("GOT NAV_SOL\n"); gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; break; } case UBX_MESSAGE_NAV_TIMEUTC: { // printf("GOT NAV_TIMEUTC\n"); gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = packet->year - 1900; timeinfo.tm_mon = packet->month - 1; timeinfo.tm_mday = packet->day; timeinfo.tm_hour = packet->hour; timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); #ifndef CONFIG_RTC //Since we lack a hardware RTC, set the system time clock based on GPS UTC //TODO generalize this by moving into gps.cpp? timespec ts; ts.tv_sec = epoch; ts.tv_nsec = packet->time_nanoseconds; clock_settime(CLOCK_REALTIME, &ts); #endif _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); _got_timeutc = true; ret = 1; break; } case UBX_MESSAGE_NAV_SVINFO: { //printf("GOT NAV_SVINFO\n"); const int length_part1 = 8; gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; const int length_part2 = 12; gps_bin_nav_svinfo_part2_packet_t *packet_part2; uint8_t satellites_used = 0; int i; //printf("Number of Channels: %d\n", packet_part1->numCh); for (i = 0; i < packet_part1->numCh; i++) { /* set pointer to sattelite_i information */ packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); /* write satellite information to global storage */ uint8_t sv_used = packet_part2->flags & 0x01; if (sv_used) { /* count SVs used for NAV */ satellites_used++; } /* record info for all channels, whether or not the SV is used for NAV */ _gps_position->satellite_used[i] = sv_used; _gps_position->satellite_snr[i] = packet_part2->cno; _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); _gps_position->satellite_prn[i] = packet_part2->svid; //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } for (i = packet_part1->numCh; i < 20; i++) { /* unused channels have to be set to zero for e.g. MAVLink */ _gps_position->satellite_prn[i] = 0; _gps_position->satellite_used[i] = 0; _gps_position->satellite_snr[i] = 0; _gps_position->satellite_elevation[i] = 0; _gps_position->satellite_azimuth[i] = 0; } _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones if (packet_part1->numCh > 0) { _gps_position->satellite_info_available = true; } else { _gps_position->satellite_info_available = false; } _gps_position->timestamp_satellites = hrt_absolute_time(); ret = 1; break; } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; _gps_position->vel_m_s = (float)packet->speed * 1e-2f; _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; _gps_position->vel_ned_valid = true; _gps_position->timestamp_velocity = hrt_absolute_time(); _rate_count_vel++; _got_velned = true; ret = 1; break; } default: break; } break; case UBX_CLASS_ACK: { /* ignore ACK when already configured */ ret = 1; break; } case UBX_CLASS_MON: { switch (_message_id) { case UBX_MESSAGE_MON_HW: { struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; _gps_position->noise_per_ms = p->noisePerMS; _gps_position->jamming_indicator = p->jamInd; ret = 1; break; } default: break; } } default: break; } if (ret == 0) { /* message not handled */ warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); hrt_abstime t = hrt_absolute_time(); if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ _disable_cmd_last = t; warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } } } else { /* handle only ACK while configuring */ if (_message_class == UBX_CLASS_ACK) { switch (_message_id) { case UBX_MESSAGE_ACK_ACK: { // printf("GOT ACK_ACK\n"); gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; if (_waiting_for_ack) { if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { _waiting_for_ack = false; ret = 1; } } break; } case UBX_MESSAGE_ACK_NAK: { // printf("GOT ACK_NAK\n"); warnx("ubx: not acknowledged"); /* configuration obviously not successful */ _waiting_for_ack = false; ret = -1; break; } default: break; } } } decode_init(); return ret; }
int main(int argc, char *argv[]) { struct sigevent ev; struct sigaction act; struct timespec tsclock, ts, tsleft, tsreset; struct itimerspec its; timer_t tid; sigset_t set; /* Check that we're root...can't call clock_settime with CLOCK_REALTIME otherwise */ if(getuid() != 0) { printf("Run this test as ROOT, not as a Regular User\n"); return PTS_UNTESTED; } /* * set up sigevent for timer * set up signal set for sigwait * set up sigaction to catch signal */ ev.sigev_notify = SIGEV_SIGNAL; ev.sigev_signo = SIGTOTEST; act.sa_handler=handler; act.sa_flags=0; if ( (sigemptyset(&set) != 0) || (sigemptyset(&act.sa_mask) != 0) ) { perror("sigemptyset() was not successful\n"); return PTS_UNRESOLVED; } if (sigaddset(&set, SIGTOTEST) != 0) { perror("sigaddset() was not successful\n"); return PTS_UNRESOLVED; } if (sigaction(SIGTOTEST, &act, 0) != 0) { perror("sigaction() was not successful\n"); return PTS_UNRESOLVED; } if (timer_create(CLOCK_REALTIME, &ev, &tid) != 0) { perror("timer_create() did not return success\n"); return PTS_UNRESOLVED; } its.it_interval.tv_sec = 0; its.it_interval.tv_nsec = 0; its.it_value.tv_sec = TIMERSEC; its.it_value.tv_nsec = 0; if (timer_settime(tid, 0, &its, NULL) != 0) { perror("timer_settime() did not return success\n"); return PTS_UNRESOLVED; } if (clock_gettime(CLOCK_REALTIME, &tsclock) != 0) { printf("clock_gettime() did not return success\n"); return PTS_UNRESOLVED; } tsclock.tv_sec -= CLOCKOFFSET; getBeforeTime(&tsreset); if (clock_settime(CLOCK_REALTIME, &tsclock) != 0) { printf("clock_settime() was not successful\n"); return PTS_UNRESOLVED; } ts.tv_sec=TIMERSEC+SLEEPDELTA; ts.tv_nsec=0; if (nanosleep(&ts, &tsleft) != -1) { printf("nanosleep() not interrupted\n"); return PTS_FAIL; } if ( abs(tsleft.tv_sec-SLEEPDELTA) <= ACCEPTABLEDELTA) { printf("Test PASSED\n"); tsreset.tv_sec += TIMERSEC; setBackTime(tsreset); return PTS_PASS; } else { printf("Timer did not last for correct amount of time\n"); printf("timer: %d != correct %d\n", (int) ts.tv_sec- (int) tsleft.tv_sec, TIMERSEC); return PTS_FAIL; } return PTS_UNRESOLVED; }
static inline int date_settime(FAR struct nsh_vtbl_s *vtbl, FAR const char *name, FAR char *newtime) { struct timespec ts; struct tm tm; FAR char *token; FAR char *saveptr; long result; int ret; /* Only this date format is supported: MMM DD HH:MM:SS YYYY */ /* Get the month abbreviation */ token = strtok_r(newtime, " \t",&saveptr); if (token == NULL) { goto errout_bad_parm; } tm.tm_mon = date_month(token); if (tm.tm_mon < 0) { goto errout_bad_parm; } /* Get the day of the month. NOTE: Accepts day-of-month up to 31 for all months */ token = strtok_r(NULL, " \t",&saveptr); if (token == NULL) { goto errout_bad_parm; } result = strtol(token, NULL, 10); if (result < 1 || result > 31) { goto errout_bad_parm; } tm.tm_mday = (int)result; /* Get the hours */ token = strtok_r(NULL, " \t:", &saveptr); if (token == NULL) { goto errout_bad_parm; } result = strtol(token, NULL, 10); if (result < 0 || result > 23) { goto errout_bad_parm; } tm.tm_hour = (int)result; /* Get the minutes */ token = strtok_r(NULL, " \t:", &saveptr); if (token == NULL) { goto errout_bad_parm; } result = strtol(token, NULL, 10); if (result < 0 || result > 59) { goto errout_bad_parm; } tm.tm_min = (int)result; /* Get the seconds */ token = strtok_r(NULL, " \t:", &saveptr); if (token == NULL) { goto errout_bad_parm; } result = strtol(token, NULL, 10); if (result < 0 || result > 61) { goto errout_bad_parm; } tm.tm_sec = (int)result; /* And finally the year */ token = strtok_r(NULL, " \t", &saveptr); if (token == NULL) { goto errout_bad_parm; } result = strtol(token, NULL, 10); if (result < 1900 || result > 2100) { goto errout_bad_parm; } tm.tm_year = (int)result - 1900; /* Convert this to the right form, then set the timer */ ts.tv_sec = mktime(&tm); ts.tv_nsec = 0; ret = clock_settime(CLOCK_REALTIME, &ts); if (ret < 0) { nsh_output(vtbl, g_fmtcmdfailed, name, "clock_settime", NSH_ERRNO); return ERROR; } return OK; errout_bad_parm: nsh_output(vtbl, g_fmtarginvalid, name); return ERROR; }
int read_data() { unsigned char buffer[BUFFER_SIZE]; oem4_header_t header; int result, read_len, rd_cnt; unsigned int crc; pushback_reader_t efd; oem4_bestpos_t bestpos; oem4_bestvel_t bestvel; gps_pos_novatel_t gps_pos; gps_vel_novatel_t gps_vel; struct timespec tloc; static int is_init = 0; //in case of read-errors from ser. device this function will be called several times in case of error from main. // if(is_init) { erret(pb_reset(&efd)); } else { erret(pb_init(fd, BUFFER_SIZE*2, &efd)); is_init = 1; } memset(buffer, 0, sizeof(buffer)); memset(&gps_pos, 0, sizeof(gps_pos)); memset(&gps_vel, 0, sizeof(gps_vel)); rd_cnt = 0; result = 0; while(1) { // read sync erret(pb_read(&efd, buffer, (unsigned int *)&read_len, OEM4_SYNC_SIZE)); if(read_len<=0) fprintf(stderr,"read timeout\r\n"); // read enough (3) sync bytes? if (read_len != OEM4_SYNC_SIZE) { // no, pusback what we had read so far and try again... erret(pb_unread(&efd, buffer, read_len)); continue; } // increment counter of read bytes rd_cnt += read_len; // sync found? if (buffer[0] != OEM4_BIN_SYNC_HEADER[0] || buffer[1] != OEM4_BIN_SYNC_HEADER[1] || buffer[2] != OEM4_BIN_SYNC_HEADER[2]) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message // if no (partial) sync message is found, nothing is unread (rd_cnt - result) == 0 erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); // everything is pushed back, therefore nothing is read rd_cnt = 0; continue; } // read head length erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, 1)); // nothing was read? if (result == 0) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt); // push back and try again erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } rd_cnt += read_len; // assign header length header.length = buffer[3]; // header too long for the buffer? if(header.length + OEM4_SYNC_SIZE >= BUFFER_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // read header, keep in mind that four bytes of the header are allready read erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.length - 4)); // read less than expected? if (read_len != header.length - 4) { // search for sync message or part of sync message result = oem4_search_sync(buffer, rd_cnt + read_len); // push back what we had and try again erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result)); rd_cnt = 0; continue; } rd_cnt += read_len; oem4_fill_header(&header, buffer); // data too long for the buffer? if ((header.message_length + header.length + OEM4_SYNC_SIZE + OEM4_CRC_SIZE) >= BUFFER_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // read data and CRC erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.message_length + OEM4_CRC_SIZE)); // read less than expected? if (read_len != header.message_length + OEM4_CRC_SIZE) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt + read_len); // push back what we had and try again erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result)); rd_cnt = 0; continue; } rd_cnt += read_len; crc32_block(buffer, rd_cnt, &crc); // CRC ok? (crc == 0) if (crc) { // this is a serious error, search for sync result = oem4_search_sync(buffer, rd_cnt); // unread everything from the beginning of the sync message erret(pb_unread(&efd, &buffer[result], rd_cnt - result)); rd_cnt = 0; continue; } // decode message switch(header.message_id) { case OEM4_BESTPOS: if (!oem4_get_best_pos(&bestpos, &buffer[header.length], header.message_length)) { #if AR_VERBOSE==1 oem4_dump_best_pos(&bestpos); #endif memcpy(&bestpos.gps_header, &header, sizeof(header)); create_gps_position(&bestpos, &gps_pos); SHM_WriteSlot(config.ishm_pos_out, &gps_pos, sizeof(gps_pos)); clock_gettime(CLOCK_REALTIME, &tloc); if(abs(tloc.tv_sec - gps_pos.gps_time.tv_sec) > 60*60*4 && gps_pos.solution_type>0) clock_settime(CLOCK_REALTIME, &gps_pos.gps_time); } break; case OEM4_BESTVEL: if (!oem4_get_best_vel(&bestvel, &buffer[header.length], header.message_length)) { memcpy(&bestvel.gps_header, &header, sizeof(header)); create_gps_velocity(&bestvel, &gps_vel); SHM_WriteSlot(config.ishm_vel_out, &gps_vel, sizeof(gps_vel)); } break; } rd_cnt = 0; } return 0; }
int main(int argc,char **argv) { SpaceFinder finder; // std::string spaceUrl = "/./helloWorldEmbeddedSpace?groups=CPP"; std::string spaceUrl = "jini://bubi3/*/mySpace"; if(argc > 1) spaceUrl = argv[1]; std::cout << "---------- Hello World running................" << std::endl; try { /////////////////////////////////////////// // retrieving a space proxy /////////////////////////////////////////// std::cout << "Retrieving a space proxy to '" << spaceUrl << "'" << std::endl; SpaceProxyPtr space ( finder.find(spaceUrl) ); if (space != NULL) std::cout << "Retrieved space proxy ok " << std::endl; else { std::cout << "ERROR: space proxy is NULL" << std::endl; std::cout << "Press Enter to exit..." << std::endl; std::cin.get(); exit(EXIT_FAILURE); } space->clear(NULL); /////////////////////////////////////////// // doing snapshot for Message /////////////////////////////////////////// Message messageTemplate; space->snapshot(&messageTemplate); std::cout << "Did snapshot for Message class" << std::endl; /////////////////////////////////////////// // writing a message to the space /////////////////////////////////////////// time_t seconds_start = time (NULL); timespec tS, dS; MessagePtr msg( new Message() ); timespec tsreq, tsrem; tsreq.tv_sec = 0; tsreq.tv_nsec = 10000000; int batch_count = 100; int write_count = 0; MessagePtr m( new Message() ); int test_duration_in_sec = 60; tS.tv_sec = 0; tS.tv_nsec = 0; // clock_settime(CLOCK_THREAD_CPUTIME_ID, &tS); clock_settime(CLOCK_MONOTONIC, &tS); clock_gettime(CLOCK_MONOTONIC, &dS); std::cout << "start time is: " << dS.tv_sec << ":" << dS.tv_nsec << std::endl; m->content = "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; m->content += "0123456789012345678901234567890123456789012345678901234567890123"; int max_batch = 4096; timespec *ptS = (timespec*) malloc(max_batch * sizeof(timespec)); int tSc = 0; // run the test for 10 seconds do { for (int i = 0; i < batch_count; i++) { m->id = i; // m->content = "Hello World"; space->write(m.get(), NULL_TX, 5000000); } write_count += batch_count; // sleep for 100 millisec - we will have 10 batched of 100 objects , effectivly close to 1000 write per sec TP // our measurment will include the sleep time. We will subtract it later nanosleep(&tsreq, &tsrem); clock_gettime(CLOCK_MONOTONIC, &tS); // std::cout << "batch time taken is: " << (tS.tv_sec - dS.tv_sec) << ":" << tS.tv_nsec << std::endl; // ignore the first 2 seconds results if (tS.tv_sec > 4) { (ptS + tSc)->tv_sec = tS.tv_sec; (ptS + tSc)->tv_nsec = tS.tv_nsec; tSc++; } } while ((tS.tv_sec - dS.tv_sec) < test_duration_in_sec ); std::cout << "Batch time taken is: " << tS.tv_sec << ":" << tS.tv_nsec << std::endl; std::cout << "Batch messages written: " << write_count << std::endl; std::cout << "Batch batches written: " << tSc << std::endl; double bps = ((double) tSc / (double) tS.tv_sec); std::cout << "Batch batches/sec: " << bps << write_count << std::endl; double tsa[tSc]; memset(tsa, 0, tSc * sizeof(double)); for (int t = 1; t < tSc; t++) { timespec dts = diff((ptS + t - 1), (ptS + t)); //double nsec = (double)dts.tv_nsec / (double)1000000000; // here we subtract the sleep time from the latency measurment double nsec = ((double)(dts.tv_nsec - tsreq.tv_nsec) / (double) 1000 / (double)batch_count); double td; for (int tt = 0; tt < tSc; tt++) { if (nsec < tsa[tt]) { td = tsa[tt]; tsa[tt] = nsec; for (int ttt = tt + 1; ttt < tSc; ttt++) { nsec = tsa[ttt]; tsa[ttt] = td; td = nsec; } tt = tSc; } else { if (tsa[tt] ==0) { tsa[tt] = nsec; tt = tSc; } } } } int step = tSc / 100; std::cout << " we have " << tSc << " data points " << " we will sample every " << step << " data point " << std::endl; std::cout << " Percentile | Latency[microsec]" << std::endl; // we have tSc data points. generate histograme int percentile = 1; for (int dtsa = 0; dtsa < tSc; dtsa++) { if (dtsa > 0) { if (dtsa % step ==0) { std::cout << percentile << " | " << tsa[dtsa] << std::endl; percentile ++; } } } free(ptS); // free(tsa); } catch(XAPException &e){ e.toLogger(); } }
TEST(time, clock_settime) { errno = 0; timespec ts; ASSERT_EQ(-1, clock_settime(-1, &ts)); ASSERT_EQ(EINVAL, errno); }
int ntp_set_tod( struct timeval *tvp, void *tzp ) { static int tod; int rc = -1; int saved_errno = 0; #ifdef DEBUG if (debug) printf("In ntp_set_tod\n"); #endif #ifdef HAVE_CLOCK_SETTIME if (rc && (SET_TOD_CLOCK_SETTIME == tod || !tod)) { struct timespec ts; /* Convert timeval to timespec */ ts.tv_sec = tvp->tv_sec; ts.tv_nsec = 1000 * tvp->tv_usec; errno = 0; rc = clock_settime(CLOCK_REALTIME, &ts); saved_errno = errno; #ifdef DEBUG if (debug) { printf("ntp_set_tod: clock_settime: %d: %s\n", rc, strerror(saved_errno)); } #endif if (!tod && !rc) tod = SET_TOD_CLOCK_SETTIME; } #endif /* HAVE_CLOCK_SETTIME */ #ifdef HAVE_SETTIMEOFDAY if (rc && (SET_TOD_SETTIMEOFDAY == tod || !tod)) { struct timeval adjtv; /* * Some broken systems don't reset adjtime() when the * clock is stepped. */ adjtv.tv_sec = adjtv.tv_usec = 0; adjtime(&adjtv, NULL); errno = 0; rc = SETTIMEOFDAY(tvp, tzp); saved_errno = errno; #ifdef DEBUG if (debug) { printf("ntp_set_tod: settimeofday: %d: %s\n", rc, strerror(saved_errno)); } #endif if (!tod && !rc) tod = SET_TOD_SETTIMEOFDAY; } #endif /* HAVE_SETTIMEOFDAY */ #ifdef HAVE_STIME if (rc && (SET_TOD_STIME == tod || !tod)) { long tp = tvp->tv_sec; errno = 0; rc = stime(&tp); /* lie as bad as SysVR4 */ saved_errno = errno; #ifdef DEBUG if (debug) { printf("ntp_set_tod: stime: %d: %s\n", rc, strerror(saved_errno)); } #endif if (!tod && !rc) tod = SET_TOD_STIME; } #endif /* HAVE_STIME */ #ifdef DEBUG if (debug) { printf("ntp_set_tod: Final result: %s: %d: %s\n", set_tod_used[tod], rc, strerror(saved_errno)); } #endif /* * Say how we're setting the time of day */ if (!rc && NULL != set_tod_using) { (*set_tod_using)(set_tod_used[tod]); set_tod_using = NULL; } if (rc) errno = saved_errno; return rc; }
int main(void) { struct sigevent ev; struct timespec tpT0, tpT2, tpreset; struct itimerspec its; timer_t tid; int delta; int sig; sigset_t set; int flags = 0; /* Check that we're root...can't call clock_settime with CLOCK_REALTIME otherwise */ if (geteuid() != 0) { printf("Run this test as ROOT, not as a Regular User\n"); return PTS_UNTESTED; } /* * set up sigevent for timer * set up signal set for sigwait */ ev.sigev_notify = SIGEV_SIGNAL; ev.sigev_signo = SIGTOTEST; if (sigemptyset(&set) != 0) { perror("sigemptyset() was not successful\n"); return PTS_UNRESOLVED; } if (sigaddset(&set, SIGTOTEST) != 0) { perror("sigaddset() was not successful\n"); return PTS_UNRESOLVED; } if (sigprocmask(SIG_BLOCK, &set, NULL) == -1) { perror("sigprocmask() failed\n"); return PTS_UNRESOLVED; } if (clock_gettime(CLOCK_REALTIME, &tpT0) != 0) { perror("clock_gettime() was not successful\n"); return PTS_UNRESOLVED; } if (timer_create(CLOCK_REALTIME, &ev, &tid) != 0) { perror("timer_create() did not return success\n"); return PTS_UNRESOLVED; } flags |= TIMER_ABSTIME; its.it_interval.tv_sec = 0; its.it_interval.tv_nsec = 0; its.it_value.tv_sec = tpT0.tv_sec + TIMEROFFSET; its.it_value.tv_nsec = tpT0.tv_nsec; if (timer_settime(tid, flags, &its, NULL) != 0) { perror("timer_settime() did not return success\n"); return PTS_UNRESOLVED; } sleep(SLEEPTIME); getBeforeTime(&tpreset); if (clock_settime(CLOCK_REALTIME, &tpT0) != 0) { perror("clock_settime() was not successful"); return PTS_UNRESOLVED; } if (sigwait(&set, &sig) == -1) { perror("sigwait() was not successful\n"); return PTS_UNRESOLVED; } if (clock_gettime(CLOCK_REALTIME, &tpT2) != 0) { printf("clock_gettime() was not successful\n"); return PTS_UNRESOLVED; } delta = tpT2.tv_sec - its.it_value.tv_sec; // add back time waited to reset value and reset time tpreset.tv_sec += tpT2.tv_sec - tpT0.tv_sec; setBackTime(tpreset); printf("delta: %d\n", delta); if ((delta <= ACCEPTABLEDELTA) && (delta >= 0)) { printf("Test PASSED\n"); return PTS_PASS; } else { printf("FAIL: Ended %d, not %d\n", (int)tpT2.tv_sec, (int)its.it_value.tv_sec); return PTS_FAIL; } printf("This code should not be executed.\n"); return PTS_UNRESOLVED; }
void setCurrentTime (const TimeValue *now) { #if defined(HAVE_CLOCK_SETTIME) && defined(CLOCK_REALTIME) const struct timespec ts = { .tv_sec = now->seconds, .tv_nsec = now->nanoseconds }; if (clock_settime(CLOCK_REALTIME, &ts) == -1) { logSystemError("clock_settime"); } #elif defined(HAVE_SETTIMEOFDAY) const struct timeval tv = { .tv_sec = now->seconds, .tv_usec = now->nanoseconds / NSECS_PER_USEC }; if (settimeofday(&tv, NULL) == -1) { logSystemError("settimeofday"); } #elif defined(__MINGW32__) TimeComponents components; expandTimeValue(now, &components); SYSTEMTIME time = { .wYear = components.year, .wMonth = components.month + 1, .wDay = components.day + 1, .wHour = components.hour, .wMinute = components.minute, .wSecond = components.second, .wMilliseconds = now->nanoseconds / NSECS_PER_MSEC }; if (!SetLocalTime(&time)) { logWindowsSystemError("SetLocalTime"); } #elif defined(HAVE_STIME) const time_t seconds = now->seconds; if (stime(&seconds) == -1) { logSystemError("stime"); } #else /* set current time */ #warning set current time not supported on this platform #endif /* get current time */ } void makeTimeValue (TimeValue *value, const TimeComponents *components) { value->nanoseconds = components->nanosecond; #if defined(GRUB_RUNTIME) value->seconds = 0; #else /* make seconds */ struct tm time = { .tm_year = components->year - 1900, .tm_mon = components->month, .tm_mday = components->day + 1, .tm_hour = components->hour, .tm_min = components->minute, .tm_sec = components->second, .tm_isdst = -1 }; value->seconds = mktime(&time); #endif /* make seconds */ } void expandTimeValue (const TimeValue *value, TimeComponents *components) { time_t seconds = value->seconds; struct tm time; localtime_r(&seconds, &time); components->nanosecond = value->nanoseconds; #if defined(GRUB_RUNTIME) components->year = time.tm.year; components->month = time.tm.month - 1; components->day = time.tm.day - 1; components->hour = time.tm.hour; components->minute = time.tm.minute; components->second = time.tm.second; #else /* expand seconds */ components->year = time.tm_year + 1900; components->month = time.tm_mon; components->day = time.tm_mday - 1; components->hour = time.tm_hour; components->minute = time.tm_min; components->second = time.tm_sec; #endif /* expand seconds */ } size_t formatSeconds (char *buffer, size_t size, const char *format, int32_t seconds) { time_t time = seconds; struct tm description; localtime_r(&time, &description); return strftime(buffer, size, format, &description); } void normalizeTimeValue (TimeValue *time) { while (time->nanoseconds < 0) { time->seconds -= 1; time->nanoseconds += NSECS_PER_SEC; } while (time->nanoseconds >= NSECS_PER_SEC) { time->seconds += 1; time->nanoseconds -= NSECS_PER_SEC; } } void adjustTimeValue (TimeValue *time, int milliseconds) { TimeValue amount = { .seconds = milliseconds / MSECS_PER_SEC, .nanoseconds = (milliseconds % MSECS_PER_SEC) * NSECS_PER_MSEC }; normalizeTimeValue(time); normalizeTimeValue(&amount); time->seconds += amount.seconds; time->nanoseconds += amount.nanoseconds; normalizeTimeValue(time); }