Home | History | Annotate | Download | only in algos 
      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 
     38 void apHubSyncReset(struct ApHubSync* sync) {
     39     sync->state = 0;
     40     if (DEBUG_SYNC) {
     41         osLog(LOG_DEBUG, "ApHub sync reset");
     42     }
     43 }
     44 
     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 
     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