1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <algos/ap_hub_sync.h>
18 #include <cpu/cpuMath.h>
19
20 #include <limits.h>
21 #include <seos.h>
22
23 #define S_IN_NS(s) (UINT64_C(1000000000)*(s))
24
25 #define SYNC_EXPIRATION S_IN_NS(50) //50 sec in ns, at max 500us diff
26 #define SYNC_WINDOW_TIMEOUT S_IN_NS(2) //2 sec in ns
27 #define SYNC_FILTER_B 8
28 #define SYNC_FILTER_A 1
29
30 #define DEBUG_SYNC false
31
32 enum ApHubSyncState {
33 NOT_INITED = 0,
34 USE_MAX,
35 USE_FILTERED
36 };
37
apHubSyncReset(struct ApHubSync * sync)38 void apHubSyncReset(struct ApHubSync* sync) {
39 sync->state = 0;
40 if (DEBUG_SYNC) {
41 osLog(LOG_DEBUG, "ApHub sync reset");
42 }
43 }
44
apHubSyncAddDelta(struct ApHubSync * sync,uint64_t apTime,uint64_t hubTime)45 void apHubSyncAddDelta(struct ApHubSync* sync, uint64_t apTime, uint64_t hubTime) {
46
47 int64_t delta = apTime - hubTime;
48
49 // if data is expired or lastTs is not set before, reset
50 if (apTime > sync->lastTs + SYNC_EXPIRATION || sync->lastTs == 0) {
51 apHubSyncReset(sync);
52 }
53
54 sync->lastTs = apTime;
55
56 if (sync->state == NOT_INITED) {
57 // setup the windowMax before switching state
58 sync->windowMax = delta;
59 sync->windowTimeout = apTime + SYNC_WINDOW_TIMEOUT;
60
61 sync->state = USE_MAX;
62 } else {
63 sync->windowMax = (delta > sync->windowMax) ? delta : sync->windowMax;
64 if (apTime > sync->windowTimeout) {
65 // collected a window
66
67 // setup deltaEstimation before switching state
68 if (sync->state == USE_MAX) {
69 sync->deltaEstimation = sync->windowMax;
70 } else {
71 sync->deltaEstimation = ((SYNC_FILTER_B - SYNC_FILTER_A) * sync->deltaEstimation +
72 SYNC_FILTER_A * sync->windowMax) / SYNC_FILTER_B;
73 }
74 sync->state = USE_FILTERED;
75 if (DEBUG_SYNC) {
76 osLog(LOG_DEBUG, "ApHub new sync offset = %" PRId64, sync->deltaEstimation);
77 }
78 // start new window by resetting windowMax and windowTimeout after this window is done
79 sync->windowMax = INT64_MIN;
80 sync->windowTimeout = apTime + SYNC_WINDOW_TIMEOUT;
81 }
82 }
83 }
84
apHubSyncGetDelta(struct ApHubSync * sync,uint64_t hubTime)85 int64_t apHubSyncGetDelta(struct ApHubSync* sync, uint64_t hubTime) {
86 int64_t ret;
87 switch (sync->state) {
88 case NOT_INITED:
89 ret = 0;
90 break;
91 case USE_MAX:
92 ret = sync->windowMax;
93 break;
94 case USE_FILTERED:
95 ret = sync->deltaEstimation;
96 break;
97 default:
98 // indicate error, should never happen
99 ret = INT64_MIN;
100 osLog(LOG_WARN, "ApHub sync: Invalid sync state %d", sync->state);
101 apHubSyncReset(sync);
102 }
103 return ret;
104 }
105
106
107