16c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen/* 26c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * Copyright (C) 2011 The Android Open Source Project 36c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * 46c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * Licensed under the Apache License, Version 2.0 (the "License"); 56c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * you may not use this file except in compliance with the License. 66c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * You may obtain a copy of the License at 76c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * 86c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * http://www.apache.org/licenses/LICENSE-2.0 96c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * 106c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * Unless required by applicable law or agreed to in writing, software 116c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * distributed under the License is distributed on an "AS IS" BASIS, 126c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 136c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * See the License for the specific language governing permissions and 146c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen * limitations under the License. 156c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen */ 166c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 176c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#ifndef __CLOCK_RECOVERY_H__ 186c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#define __CLOCK_RECOVERY_H__ 196c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 206c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#include <stdint.h> 216c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#include <common_time/ICommonClock.h> 226c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#include <utils/LinearTransform.h> 236c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#include <utils/threads.h> 246c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 256c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#ifdef TIME_SERVICE_DEBUG 266c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#include "diag_thread.h" 276c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#endif 286c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 296c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chennamespace android { 306c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 316c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chenclass CommonClock; 326c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chenclass LocalClock; 336c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 346c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chenclass ClockRecoveryLoop { 356c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen public: 366c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen ClockRecoveryLoop(LocalClock* local_clock, CommonClock* common_clock); 376c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen ~ClockRecoveryLoop(); 386c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 396c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen void reset(bool position, bool frequency); 406c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen bool pushDisciplineEvent(int64_t local_time, 416c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t nominal_common_time, 426c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t data_point_rtt); 436c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int32_t getLastErrorEstimate(); 446c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 456c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen private: 4611bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 4711bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Tuned using the "Good Gain" method. 4811bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // See: 4911bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // http://techteach.no/publications/books/dynamics_and_control/tuning_pid_controller.pdf 5011bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 5111bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Controller period (1Hz for now). 5211bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float dT; 5311bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 5411bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Controller gain, positive and unitless. Larger values converge faster, 5511bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // but can cause instability. 5611bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float Kc; 5711bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 5811bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Integral reset time. Smaller values cause loop to track faster, but can 5911bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // also cause instability. 6011bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float Ti; 6111bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 6211bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Controller output filter time constant. Range (0-1). Smaller values make 6311bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // output smoother, but slow convergence. 6411bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float Tf; 6511bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 6611bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Low-pass filter for bias tracker. 6711bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float bias_Fc; // HZ 6811bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float bias_RC; // Computed in constructor. 6911bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float bias_Alpha; // Computed inconstructor. 7011bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 7111bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // The maximum allowed error (as indicated by a pushDisciplineEvent) before 7211bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // we panic. 7311bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const int64_t panic_thresh_; 7411bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 7511bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // The maximum allowed error rtt time for packets to be used for control 7611bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // feedback, unless the packet is the best in recent memory. 7711bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const int64_t control_thresh_; 786c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 796c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen typedef struct { 806c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t local_time; 816c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t observed_common_time; 826c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t nominal_common_time; 836c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int64_t rtt; 846c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen bool point_used; 856c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen } DisciplineDataPoint; 866c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 876c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen static uint32_t findMinRTTNdx(DisciplineDataPoint* data, uint32_t count); 886c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 896c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen void reset_l(bool position, bool frequency); 906c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen void applySlew(); 916c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 926c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // The local clock HW abstraction we use as the basis for common time. 936c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen LocalClock* local_clock_; 946c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen bool local_clock_can_slew_; 956c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 966c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // The common clock we end up controlling along with the lock used to 976c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // serialize operations. 986c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen CommonClock* common_clock_; 996c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen Mutex lock_; 1006c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 1016c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // parameters maintained while running and reset during a reset 1026c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // of the frequency correction. 1036c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen bool last_delta_valid_; 1046c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int32_t last_delta_; 10511bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk float last_delta_f_; 1066c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int32_t integrated_error_; 1076c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen int32_t correction_cur_; 1086c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 10911bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Contoller Output. 11011bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk float CO; 11111bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 11211bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Bias tracking for trajectory estimation. 11311bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk float CObias; 11411bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk float lastCObias; 11511bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 11611bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // Controller output bounds. The controller will not try to 11711bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk // slew faster that +/-100ppm offset from center per interation. 11811bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float COmin; 11911bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const float COmax; 12011bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk 1216c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen // State kept for filtering the discipline data. 12211bc45fcba96cf7ccc5f67b3c47088c2c89c8e7aKent Ryhorchuk static const uint32_t kFilterSize = 16; 1236c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen DisciplineDataPoint filter_data_[kFilterSize]; 1246c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen uint32_t filter_wr_; 1256c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen bool filter_full_; 1266c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 1276c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen static const uint32_t kStartupFilterSize = 4; 1286c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen DisciplineDataPoint startup_filter_data_[kStartupFilterSize]; 1296c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen uint32_t startup_filter_wr_; 1306c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 1316c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#ifdef TIME_SERVICE_DEBUG 1326c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen sp<DiagThread> diag_thread_; 1336c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#endif 1346c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen}; 1356c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 1366c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen} // namespace android 1376c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen 1386c929510474caa14dc9d56826b2c65552861d6b3Mike J. Chen#endif // __CLOCK_RECOVERY_H__ 139