Statistics
| Branch: | Revision:

napa-baselibs / monl / plugins / clockdrift_measure.cpp @ 507372bb

History | View | Annotate | Download (6.66 KB)

1
/***************************************************************************
2
 *   Copyright (C) 2009 by Robert Birke
3
 *   robert.birke@polito.it
4
 *
5
 * This library is free software; you can redistribute it and/or
6
 * modify it under the terms of the GNU Lesser General Public
7
 * License as published by the Free Software Foundation; either
8
 * version 2.1 of the License, or (at your option) any later version.
9

10
 * This library is distributed in the hope that it will be useful,
11
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13
 * Lesser General Public License for more details.
14

15
 * You should have received a copy of the GNU Lesser General Public
16
 * License along with this library; if not, write to the Free Software
17
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA 
18
 ***********************************************************************/
19

    
20
#include "clockdrift_measure.h"
21
#include "napa_log.h"
22
#include <math.h>
23

    
24

    
25
double ClockdriftMeasure::x_axis_inter(double a1, double b1, double a2, double b2) {
26
        return((b2-b1)/(a1-a2));
27
}
28

    
29
double ClockdriftMeasure::y_axis_inter(double a1, double b1, double a2, double b2) {
30
        return a1 * x_axis_inter(a1, b1, a2, b2) + b1;
31
}
32

    
33
bool clockdrift_sort_function (struct clockdrift_sample i,struct clockdrift_sample j)
34
{ return (i.tx_time<j.tx_time); }
35

    
36
ClockdriftMeasure::ClockdriftMeasure(class MeasurePlugin *m, MeasurementCapabilities mc, class MeasureDispatcher *md): MonMeasure(m,mc,md) {
37
        if(param_values[P_CLOCKDRIFT_ALGORITHM] != 1) {
38
                n = new int[(int)param_values[P_CLOCKDRIFT_WIN_SIZE]];
39
                samples.reserve((int)param_values[P_CLOCKDRIFT_WIN_SIZE]);
40
        } else
41
                n = NULL;
42
        a = 0;
43
        b = 0;
44
        first_tx = 0;
45
}
46

    
47
ClockdriftMeasure::~ClockdriftMeasure() {
48
        if(n != NULL) {
49
                delete[] n;
50
                n = NULL;
51
        }
52
}
53

    
54

    
55
void ClockdriftMeasure::init() {
56
        samples.clear();
57
        first_tx = NAN;
58
        pos = 0;
59
        pnum = 0;
60
        sum_t = sum_d = sum_t_v = sum_d_v = sum_td_v = 0;
61
}
62

    
63
void ClockdriftMeasure::stop() {
64
        r_tx_list[R_CLOCKDRIFT] = r_rx_list[R_CLOCKDRIFT] = NAN;
65
}
66

    
67
int ClockdriftMeasure::paramChange(MonParameterType ph, MonParameterValue p){
68
                switch(ph) {
69
                case P_CLOCKDRIFT_ALGORITHM:
70
                        if(p != 1) {
71
                                if(n == NULL) {
72
                                        n = new int[(int)param_values[P_CLOCKDRIFT_WIN_SIZE]];
73
                                        samples.reserve((int)param_values[P_CLOCKDRIFT_WIN_SIZE]);
74
                                        //TODO: possibly? keep old data
75
                                        samples.clear();
76
                                        first_tx = NAN;
77
                                        pos = 0;
78
                                        pnum = 0;
79
                                        sum_t = sum_d = sum_t_v = sum_d_v = sum_td_v = 0;
80
                                }
81
                        } else {
82
                                if(n != NULL) {
83
                                        delete[] n;
84
                                        n = NULL;
85
                                }
86
                        }
87
                        return EOK;
88
                case P_CLOCKDRIFT_WIN_SIZE:
89
                        if(n != NULL) {
90
                                delete[] n;
91
                                n = new int[(int)p];
92
                                samples.reserve((int)p);
93
                        }
94
                        //TODO: possibly? keep old data
95
                        samples.clear();
96
                        first_tx = NAN;
97
                        pos = 0;
98
                        pnum = 0;
99
                        sum_t = sum_d = sum_t_v = sum_d_v = sum_td_v = 0;
100
                        return EOK;
101
                default:
102
                        return EOK;
103
        }
104
}
105

    
106
result ClockdriftMeasure::RxPkt(result *r,ExecutionList *el) {
107
        double tx_time, delay;
108
        result res = NAN;
109

    
110
        if(isnan(first_tx))
111
                first_tx = r[R_SEND_TIME];
112

    
113
        tx_time = r[R_SEND_TIME] - first_tx;
114
        delay = fabs(r[R_SEND_TIME] - r[R_RECEIVE_TIME]);
115
        pnum++;
116

    
117
        if(param_values[P_CLOCKDRIFT_ALGORITHM] == 1 || param_values[P_CLOCKDRIFT_ALGORITHM] == 0) {
118
                sum_t += tx_time;
119
                sum_t_v += pow(tx_time - sum_t/pnum, 2);
120
                sum_d += delay;
121
                sum_d_v += pow(delay - sum_d/pnum, 2);
122
                sum_td_v += (delay - sum_d/pnum) * (tx_time - sum_t/pnum);
123
        }
124

    
125
        if(param_values[P_CLOCKDRIFT_ALGORITHM] != 1) {
126
                samples[pos].tx_time = tx_time;
127
                samples[pos].delay = delay;
128
        }
129

    
130
        pos++;
131

    
132
        if(pos % (int)param_values[P_CLOCKDRIFT_PKT_TH] == 0) {
133
                int i,j,k,end;
134
                double m_t, m_d, v_t, v_d, v_td, sum;
135
                m_t = m_d = 0;
136

    
137
                if(param_values[P_CLOCKDRIFT_ALGORITHM] == 1 || param_values[P_CLOCKDRIFT_ALGORITHM] == 0) {
138
                        r[R_CLOCKDRIFT] = b = sum_td_v / sum_t_v;
139
                        a = sum_d/pnum - b * sum_t/pnum;
140

    
141
                        char dbg[512];
142
                        snprintf(dbg, sizeof(dbg), "Ts: %f Clockdrift 1 a: %f b: %f", r[R_RECEIVE_TIME], a, b);
143
                        debugOutput(dbg);
144
                }
145

    
146
                end = pnum > (int)param_values[P_CLOCKDRIFT_WIN_SIZE] ? (int)param_values[P_CLOCKDRIFT_WIN_SIZE] : pos;
147

    
148
                if(param_values[P_CLOCKDRIFT_ALGORITHM] == 2 || param_values[P_CLOCKDRIFT_ALGORITHM] == 0) {
149
                        for(i = 0; i < end; i++) {
150
                                m_t += samples[i].tx_time;
151
                                m_d += samples[i].delay;
152
                        }
153
                        m_t = m_t /i; m_d = m_d /i;
154
        
155
                        v_t = v_d = v_td = 0;
156
                        for(i = 0; i < end; i++) {
157
                                v_t += pow(samples[i].tx_time - m_t, 2);
158
                                v_d += pow(samples[i].delay - m_d, 2);
159
                                v_td += (samples[i].tx_time - m_t) * (samples[i].delay - m_d);
160
                        }
161
        
162
                        r[R_CLOCKDRIFT] = b = v_td / v_t;
163
                        a = m_d - b * m_t;
164

    
165
                        char dbg[512];
166
                        snprintf(dbg, sizeof(dbg), "Ts: %f Clockdrift 2 a: %f b: %f", r[R_RECEIVE_TIME], a, b);
167
                        debugOutput(dbg);
168
                }
169

    
170
                if(param_values[P_CLOCKDRIFT_ALGORITHM] == 3 || param_values[P_CLOCKDRIFT_ALGORITHM] == 0) {
171
                        std::sort(samples.begin(), samples.begin() + end, clockdrift_sort_function);
172
        
173
                        /* start estimation */
174
                        n[0] = 0; n[1] = 1; k = 1;
175
                        for (i = 2; i < end; i++) {
176
                                for (j = k; j >= 1; j--) {
177
                                        if (x_axis_inter(samples[i].tx_time, -samples[i].delay, samples[n[j]].tx_time, -samples[n[j]].delay)
178
                                                        < x_axis_inter(samples[n[j]].tx_time, -samples[n[j]].delay,samples[n[j-1]].tx_time, -samples[n[j-1]].delay))
179
                                                break;
180
                                }
181
                                k = j + 1;
182
                                n[k] = i;
183
                        }
184
        
185
                        sum = 0.0;
186
                        for (i = 0; i < end; i++)
187
                                sum += samples[i].tx_time;
188
        
189
                        sum /= end;
190
                        for (i = 0; i < k - 1; i++) {
191
                                if (samples[n[i]].tx_time< sum && sum <= samples[n[i+1]].tx_time) {
192
                                        r[R_CLOCKDRIFT] = b = x_axis_inter(samples[n[i]].tx_time, -samples[n[i]].delay, samples[n[i+1]].tx_time, -samples[n[i+1]].delay);
193
                                        a = -y_axis_inter(samples[n[i]].tx_time, -samples[n[i]].delay, samples[n[i+1]].tx_time, -samples[n[i+1]].delay);
194
                                        break;
195
                                }
196
                        }
197

    
198
                        char dbg[512];
199
                        snprintf(dbg, sizeof(dbg), "Ts: %f Clockdrift 3 a: %f b: %f", r[R_RECEIVE_TIME], a, b);
200
                        debugOutput(dbg);
201
                }
202
                res = r[R_CLOCKDRIFT];
203
        }
204

    
205
        if(pos == param_values[P_CLOCKDRIFT_WIN_SIZE])
206
                pos = 0;
207

    
208
        return res;
209
}
210

    
211
result ClockdriftMeasure::RxData(result *r,ExecutionList *el) {
212
        return RxPkt(r, el);
213
}
214

    
215
ClockdriftMeasurePlugin::ClockdriftMeasurePlugin() {
216
        /* Initialise properties: MANDATORY! */
217
        name = "Clock Drift";
218
        desc = "The drift between the clocks of the two systems";
219
        id = CLOCKDRIFT;
220
        /* end of mandatory properties */
221
        addParameter(new MinMaxParameter("Algorithm","1 - Linear regression (interactive), 2 - Linear regression, 3 - Linear programming",0,3,0), P_CLOCKDRIFT_ALGORITHM);
222
        addParameter(new MinParameter("Packet threshold","How often to compute the drift",100,100), P_CLOCKDRIFT_PKT_TH);
223
        addParameter(new MinParameter("Window size","The window size on which to compute",100,1000), P_CLOCKDRIFT_WIN_SIZE);
224
}