src/sensor/pedometer/sensor_frequency_compensator.cpp

   1 /*
   2  *  Copyright (c) 2016-2017 Samsung Electronics Co., Ltd.
   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 "sensor_frequency_compensator.h"
  18
  19 #include <stdlib.h>
  20 #include <math.h>
  21 #include <string.h>
  22
  23 /************************************************************************
  24  */
  25 sensor_frequency_compensator::sensor_frequency_compensator(double desired_rate)
  26 : m_desired_frequency(desired_rate)
  27 , m_t1(0)
  28 , m_v1{0.0, 0.0, 0.0}
  29 , m_t2(0)
  30 , m_v2{0.0, 0.0, 0.0}
  31 , m_timestamp(0)
  32 {
  33 }
  34
  35 /************************************************************************
  36  */
  37 sensor_frequency_compensator::~sensor_frequency_compensator()
  38 {
  39 }
  40
  41 /************************************************************************
  42  */
  43 void sensor_frequency_compensator::reset() {
  44         m_t1 = UNKNOWN_TIMESTAMP;
  45         m_t2 = UNKNOWN_TIMESTAMP;
  46         m_timestamp = UNKNOWN_TIMESTAMP;
  47 }
  48
  49 /************************************************************************
  50  */
  51 void sensor_frequency_compensator::add(timestamp_t t, double *v) {
  52         if (m_timestamp == UNKNOWN_TIMESTAMP) {
  53                 m_timestamp = t;
  54         }
  55         m_t1 = m_t2;
  56         memcpy(m_v1, m_v2, sizeof(m_v1));
  57         m_t2 = t;
  58         memcpy(m_v2, v, sizeof(m_v2));
  59 }
  60
  61 /************************************************************************
  62  */
  63 bool sensor_frequency_compensator::has_next() {
  64         if (m_t1 == UNKNOWN_TIMESTAMP || m_t2 == UNKNOWN_TIMESTAMP) {
  65                 return false;
  66         }
  67         return m_timestamp + m_desired_frequency < m_t2;
  68 }
  69
  70 /************************************************************************
  71  */
  72 void sensor_frequency_compensator::get_next(double *v) {
  73         timestamp_t t3 = m_timestamp;
  74         if (t3 < m_t1) {
  75                 t3 = m_t1;
  76         }
  77         m_timestamp += m_desired_frequency;
  78         double t = (t3 - m_t1) / ((double) (m_t2 - m_t1));
  79         int i;
  80         for (i = 0; i < 3; i++) {
  81                 v[i] = (1.0 - t) * m_v1[i] + t * m_v2[i];
  82         }
  83 }