mirror of
https://github.com/TomHarte/CLK.git
synced 2024-11-19 23:32:28 +00:00
74 lines
2.1 KiB
C++
74 lines
2.1 KiB
C++
//
|
|
// DigitalPhaseLockedLoop.cpp
|
|
// Clock Signal
|
|
//
|
|
// Created by Thomas Harte on 11/07/2016.
|
|
// Copyright © 2016 Thomas Harte. All rights reserved.
|
|
//
|
|
|
|
#include "DigitalPhaseLockedLoop.hpp"
|
|
#include <algorithm>
|
|
#include <cstdlib>
|
|
|
|
using namespace Storage;
|
|
|
|
DigitalPhaseLockedLoop::DigitalPhaseLockedLoop(int clocks_per_bit, size_t length_of_history) :
|
|
clocks_per_bit_(clocks_per_bit),
|
|
phase_(0),
|
|
window_length_(clocks_per_bit),
|
|
offset_history_pointer_(0),
|
|
offset_history_(length_of_history, 0),
|
|
offset_(0),
|
|
delegate_(nullptr) {}
|
|
|
|
void DigitalPhaseLockedLoop::run_for(const Cycles cycles) {
|
|
offset_ += cycles.as_int();
|
|
phase_ += cycles.as_int();
|
|
if(phase_ >= window_length_) {
|
|
int windows_crossed = phase_ / window_length_;
|
|
|
|
// check whether this triggers any 0s, if anybody cares
|
|
if(delegate_) {
|
|
if(window_was_filled_) windows_crossed--;
|
|
for(int c = 0; c < windows_crossed; c++)
|
|
delegate_->digital_phase_locked_loop_output_bit(0);
|
|
}
|
|
|
|
window_was_filled_ = false;
|
|
phase_ %= window_length_;
|
|
}
|
|
}
|
|
|
|
void DigitalPhaseLockedLoop::add_pulse() {
|
|
if(!window_was_filled_) {
|
|
if(delegate_) delegate_->digital_phase_locked_loop_output_bit(1);
|
|
window_was_filled_ = true;
|
|
post_phase_offset(phase_, offset_);
|
|
offset_ = 0;
|
|
}
|
|
}
|
|
|
|
void DigitalPhaseLockedLoop::post_phase_offset(int new_phase, int new_offset) {
|
|
offset_history_[offset_history_pointer_] = new_offset;
|
|
offset_history_pointer_ = (offset_history_pointer_ + 1) % offset_history_.size();
|
|
|
|
// use an unweighted average of the stored offsets to compute current window size,
|
|
// bucketing them by rounding to the nearest multiple of the base clocks per bit
|
|
int total_spacing = 0;
|
|
int total_divisor = 0;
|
|
for(int offset : offset_history_) {
|
|
int multiple = (offset + (clocks_per_bit_ >> 1)) / clocks_per_bit_;
|
|
if(!multiple) continue;
|
|
total_divisor += multiple;
|
|
total_spacing += offset;
|
|
}
|
|
if(total_divisor) {
|
|
window_length_ = total_spacing / total_divisor;
|
|
}
|
|
|
|
int error = new_phase - (window_length_ >> 1);
|
|
|
|
// use a simple spring mechanism as a lowpass filter for phase
|
|
phase_ -= (error + 1) >> 1;
|
|
}
|