napabaselibs / 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 021101301 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((b2b1)/(a1a2));

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[j1]].tx_time, samples[n[j1]].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 
} 