clock.c 2.53 KB
Newer Older
1
#include <sys/time.h>
2
#include <stdlib.h>
Andreas Schmidt's avatar
Andreas Schmidt committed
3
#include "../util/time.h"
4
#include "../util/common.h"
5 6
#include "clock.h"

7
prrtTimestamp_t PrrtClock_get_current_time_us()
8
{
9 10
    struct timeval tv;
    gettimeofday(&tv, NULL);
11
    return (prrtTimestamp_t) (1000000 * tv.tv_sec + tv.tv_usec);
12
}
13

Andreas Schmidt's avatar
Andreas Schmidt committed
14
prrtTimestamp_t PrrtClock_get_prrt_time_us(PrrtClock *clock)
15
{
Andreas Schmidt's avatar
Andreas Schmidt committed
16 17
    prrtTimestamp_t currentTime = PrrtClock_get_current_time_us();
    prrtTimestamp_t virtualTime = clock->virtualTime;
18 19

    if(virtualTime) {
Andreas Schmidt's avatar
Andreas Schmidt committed
20
        prrtTimestamp_t lastMeasurement = clock->lastMeasurement;
Andreas Schmidt's avatar
Andreas Schmidt committed
21 22
        prrtTimeDifference_t diff = diff_abs_ts(currentTime, lastMeasurement);
        prrtTimeDifference_t skew = (diff * clock->skew) / 400;
Andreas Schmidt's avatar
Andreas Schmidt committed
23
        return (prrtTimestamp_t) (virtualTime + clock->meanDeviation + skew);
24 25 26
    } else {
        return currentTime;
    }
27
}
28

Andreas Schmidt's avatar
Andreas Schmidt committed
29
bool PrrtClock_update(PrrtClock *clock, uint32_t referenceTime, uint32_t propagation_delay)
30
{
Andreas Schmidt's avatar
Andreas Schmidt committed
31
    prrtTimestamp_t virtualTime = PrrtClock_get_prrt_time_us(clock);
Andreas Schmidt's avatar
Andreas Schmidt committed
32
    prrtTimestamp_t currentTime = PrrtClock_get_current_time_us();
Andreas Schmidt's avatar
Andreas Schmidt committed
33 34 35 36
    int64_t clockSkew = clock->skew;
    prrtTimedelta_t delay = propagation_delay / 2; // half the rtt
    int64_t phaseError = PrrtTimestamp_cmp(referenceTime, virtualTime) + delay;
    //printf("V: %13u, C: %13u, S: %13d, R: %13u, E: %13ld, D: %3u L: %u\n", virtualTime, currentTime, clockSkew, referenceTime, phaseError, delay, clock->lastMeasurement);
37

Andreas Schmidt's avatar
Andreas Schmidt committed
38
    if(labs(phaseError) > 10000) {
39 40 41 42 43 44
        clock->meanDeviation = 0;
        clock->skew = 0;
        clock->lastMeasurement = currentTime;
        clock->virtualTime = currentTime;
    }

Andreas Schmidt's avatar
Andreas Schmidt committed
45
    int64_t deviationSum =(phaseError + 3 * clock->meanDeviation); // TODO: why 3?
Andreas Schmidt's avatar
Andreas Schmidt committed
46
    prrtTimeDifference_t meanDeviation = (prrtTimeDifference_t) (deviationSum / 4); // TODO: find out why???
Andreas Schmidt's avatar
Andreas Schmidt committed
47
    int64_t period = PrrtTimestamp_cmp(currentTime, clock->lastMeasurement);
48 49

    if(period > 0) {
Andreas Schmidt's avatar
Andreas Schmidt committed
50
        clockSkew = (meanDeviation / period) + 15 * clock->skew;
Andreas Schmidt's avatar
Andreas Schmidt committed
51
        clockSkew = (prrtTimeDifference_t) (clockSkew / 16);
52 53
    }

Andreas Schmidt's avatar
Andreas Schmidt committed
54
    virtualTime = clock->virtualTime + meanDeviation + period * clockSkew / 400;
55 56

    clock->meanDeviation = meanDeviation;
Andreas Schmidt's avatar
Andreas Schmidt committed
57
    clock->skew = (prrtTimeDifference_t) clockSkew;
58 59 60
    clock->lastMeasurement = currentTime;
    clock->virtualTime = virtualTime;

Andreas Schmidt's avatar
Andreas Schmidt committed
61
    //printf("V: %13u, C: %13u; S: %13d; M: %13d, P:%13u\n\n", virtualTime, currentTime, clockSkew, meanDeviation, period);
Andreas Schmidt's avatar
Andreas Schmidt committed
62
    return true;
63 64
}

Stefan Reif's avatar
Stefan Reif committed
65
void PrrtClock_init(PrrtClock *clock)
66
{
67 68 69
    clock->meanDeviation = 0;
    clock->virtualTime = 0;
    clock->lastMeasurement = 0;
Andreas Schmidt's avatar
Andreas Schmidt committed
70
    clock->skew = 0;
Stefan Reif's avatar
Stefan Reif committed
71 72
}