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 <math.h>
     18 #include <floatRt.h>
     19 #include <algos/time_sync.h>
     20 
     21 void time_sync_reset(time_sync_t *sync) {
     22     sync->n = 0;
     23     sync->i = 0;
     24     sync->estimate_valid = false;
     25 
     26     sync->hold_count = 0;
     27 }
     28 
     29 bool time_sync_init(time_sync_t *sync) {
     30     time_sync_reset(sync);
     31 
     32     return true;
     33 }
     34 
     35 void time_sync_truncate(time_sync_t *sync, size_t window_size) {
     36     size_t k, m;
     37     sync->n = (window_size < sync->n) ? window_size : sync->n;
     38     sync->estimate_valid = false;
     39 
     40     // oldest sample index (new time_base) after truncation
     41     size_t bidx = (sync->i >= sync->n) ? (sync->i - sync->n)
     42         : (sync->i + NUM_TIME_SYNC_DATAPOINTS - sync->n);
     43 
     44     // left circular-shift oldest sample to index 0
     45     for (k = 0; k < bidx; ++k) {
     46         uint64_t tmp1 = sync->time1[0];
     47         uint64_t tmp2 = sync->time2[0];
     48 
     49         for (m = 0; m < NUM_TIME_SYNC_DATAPOINTS - 1; ++m) {
     50             sync->time1[m] = sync->time1[m + 1];
     51             sync->time2[m] = sync->time2[m + 1];
     52         }
     53         sync->time1[NUM_TIME_SYNC_DATAPOINTS - 1] = tmp1;
     54         sync->time2[NUM_TIME_SYNC_DATAPOINTS - 1] = tmp2;
     55     }
     56 
     57     sync->i = (sync->n < NUM_TIME_SYNC_DATAPOINTS) ? sync->n : 0;
     58 }
     59 
     60 bool time_sync_add(time_sync_t *sync, uint64_t time1, uint64_t time2) {
     61     size_t i = sync->i;
     62 
     63     sync->time1[i] = time1;
     64     sync->time2[i] = time2;
     65 
     66     if (++i == NUM_TIME_SYNC_DATAPOINTS) {
     67         i = 0;
     68     }
     69 
     70     sync->i = i;
     71 
     72     size_t prev_n = sync->n;
     73     if (sync->n < NUM_TIME_SYNC_DATAPOINTS) {
     74         ++sync->n;
     75     }
     76 
     77     sync->estimate_valid = false;
     78 
     79     if (sync->hold_count > 0) {
     80         --sync->hold_count;
     81         time_sync_truncate(sync, prev_n);
     82     }
     83 
     84     return true;
     85 }
     86 
     87 bool time_sync_estimate_time1(time_sync_t *sync, uint64_t time2, uint64_t *time1)
     88 {
     89     size_t j;
     90 
     91     if (sync->n < 2)
     92         return false;
     93 
     94     *time1 = 0;
     95 
     96     if (!sync->estimate_valid) {
     97         size_t n = sync->n;
     98 
     99         // Rewind to the oldest sample in the history.
    100         size_t i = sync->i;
    101         if (n < NUM_TIME_SYNC_DATAPOINTS) {
    102             if (i != n) {
    103                 return false;
    104             }
    105             i = 0;
    106         }
    107 
    108         uint64_t time1_base = sync->time1[i];
    109         uint64_t time2_base = sync->time2[i];
    110 
    111         // Least-square linear regresison:
    112         // Compute alpha and beta so that time1 = alpha + beta * time2.
    113         // x = time2, y = time1
    114         float mean_x = 0.0f;
    115         float mean_y = 0.0f;
    116         float invN = 1.0f / n;
    117         size_t ii = i;
    118         for (j = 0; j < n; ++j) {
    119             mean_y += floatFromUint64(sync->time1[ii] - time1_base) * invN;
    120             mean_x += floatFromUint64(sync->time2[ii] - time2_base) * invN;
    121 
    122             if (++ii == NUM_TIME_SYNC_DATAPOINTS) {
    123                 ii = 0;
    124             }
    125         }
    126 
    127         // Two-pass approach so that only values relative to mean are computed.
    128         // Typically, |y| and |x| are smaller than 8e8 in nsec.
    129         // So sum_x2 and sum_xy are smaller than 1e19.
    130         // That leaves plenty of room for blocking tasks.
    131         float sum_x2 = 0.0f, sum_xy = 0.0f;
    132         ii = i;
    133         for (j = 0; j < n; ++j) {
    134             float y = floatFromUint64(sync->time1[ii] - time1_base) - mean_y;
    135             float x = floatFromUint64(sync->time2[ii] - time2_base) - mean_x;
    136 
    137             sum_x2 += x * x;
    138             sum_xy += x * y;
    139 
    140             if (++ii == NUM_TIME_SYNC_DATAPOINTS) {
    141                 ii = 0;
    142             }
    143         }
    144 
    145         float beta = sum_xy / sum_x2;
    146         float alpha = mean_y - beta * mean_x;
    147 
    148         sync->alpha = alpha;
    149         sync->beta = beta;
    150         sync->time1_base = time1_base;
    151         sync->time2_base = time2_base;
    152 
    153         sync->estimate_valid = true;
    154     }
    155 
    156     *time1 = sync->time1_base + floatToInt64(sync->alpha + sync->beta * floatFromInt64(time2 - sync->time2_base));
    157 
    158     return true;
    159 }
    160 
    161 void time_sync_hold(time_sync_t *sync, uint8_t count) {
    162     sync->hold_count = count;
    163 }
    164