1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
|
#include "../include/disambiguator.h"
#include <stdlib.h>
#include <string.h>
void disambiguator_init(disambiguator * d) {
memset(&(d->times), 0x0, sizeof(d->times));
memset(&(d->scores), 0x0, sizeof(d->scores));
d->state = D_STATE_UNLOCKED;
d->last = 0;
d->max_confidence = 0;
}
inline void disambiguator_discard(disambiguator * d, long age);
void disambiguator_discard(disambiguator * d, long age) {
int confidence = 0;
for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
if (d->times[i] != 0 && d->times[i] < age) {
d->times[i] = 0;
d->scores[i] = 0;
} else {
if (d->scores[i] > confidence) {
confidence = d->scores[i];
}
}
}
d->max_confidence = confidence;
}
inline int disambiguator_find_nearest(disambiguator * d, long time, int max_diff);
int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) {
int diff = max_diff; // max allowed diff for a match
int idx = -1;
for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
if (d->times[i] == 0) continue;
int a = abs(d->times[i] - time);
if (a < diff) {
idx = i;
diff = a;
}
}
return idx;
}
pulse_type disambiguator_step(disambiguator * d, long time, int length) {
if (length < 2750) {
return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
}
disambiguator_discard(d, time - 10000000);
int idx = disambiguator_find_nearest(d, time - 400000, 100);
if (time - d->last > 401000) {
d->state = D_STATE_UNLOCKED;
}
if (idx == -1) {
for (int i = 0; i < DIS_NUM_VALUES; ++i) {
if (d->times[i] == 0) {
d->times[i] = time;
break;
}
}
return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
} else {
d->scores[idx]++;
if (d->scores[idx] >= 30) {
d->state = D_STATE_LOCKED;
}
d->times[idx] = time;
d->last = time;
return d->state == D_STATE_LOCKED ? (
d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP
) : P_UNKNOWN;
}
return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
}
|
