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