mirror of
https://github.com/TomHarte/CLK.git
synced 2024-11-30 04:50:08 +00:00
78 lines
2.1 KiB
C++
78 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, int tolerance, size_t length_of_history) :
|
|
_clocks_per_bit(clocks_per_bit),
|
|
_tolerance(tolerance),
|
|
|
|
_phase(0),
|
|
_window_length(clocks_per_bit),
|
|
|
|
_phase_error_pointer(0)
|
|
{
|
|
_phase_error_history.reset(new std::vector<int>(length_of_history, 0));
|
|
}
|
|
|
|
void DigitalPhaseLockedLoop::run_for_cycles(int number_of_cycles)
|
|
{
|
|
_phase += number_of_cycles;
|
|
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_error(_phase - (_window_length >> 1));
|
|
}
|
|
}
|
|
|
|
void DigitalPhaseLockedLoop::post_phase_error(int error)
|
|
{
|
|
// use a simple spring mechanism as a lowpass filter for phase
|
|
_phase -= (error + 1) >> 1;
|
|
|
|
// use the average of the last few errors to affect frequency
|
|
std::vector<int> *phase_error_history = _phase_error_history.get();
|
|
size_t phase_error_history_size = phase_error_history->size();
|
|
|
|
(*phase_error_history)[_phase_error_pointer] = error;
|
|
_phase_error_pointer = (_phase_error_pointer + 1)%phase_error_history_size;
|
|
|
|
int total_error = 0;
|
|
for(size_t c = 0; c < phase_error_history_size; c++)
|
|
{
|
|
total_error += (*phase_error_history)[c];
|
|
}
|
|
int denominator = (int)(phase_error_history_size * 4);
|
|
_window_length += (total_error + (denominator >> 1)) / denominator;
|
|
_window_length = std::max(std::min(_window_length, _clocks_per_bit + _tolerance), _clocks_per_bit - _tolerance);
|
|
}
|