diff --git a/Storage/Disk/DigitalPhaseLockedLoop.cpp b/Storage/Disk/DigitalPhaseLockedLoop.cpp index 5d1319fc3..358575500 100644 --- a/Storage/Disk/DigitalPhaseLockedLoop.cpp +++ b/Storage/Disk/DigitalPhaseLockedLoop.cpp @@ -7,3 +7,58 @@ // #include "DigitalPhaseLockedLoop.hpp" + +using namespace Storage; + +DigitalPhaseLockedLoop::DigitalPhaseLockedLoop(unsigned int clocks_per_bit, unsigned int tolerance, unsigned int length_of_history) : + _clocks_per_bit(clocks_per_bit), + _tolerance(tolerance), + _length_of_history(length_of_history), + _pulse_history(new unsigned int[length_of_history]), + _current_window_length(clocks_per_bit), + _next_pulse_time(0), + _window_was_filled(false), + _window_offset(0) {} + +void DigitalPhaseLockedLoop::run_for_cycles(unsigned int number_of_cycles) +{ + // check whether this triggers any 0s + _window_offset += number_of_cycles; + if(_delegate) + { + while(_window_offset > _current_window_length) + { + if(!_window_was_filled) _delegate->digital_phase_locked_loop_output_bit(0); + _window_was_filled = false; + _window_offset -= _current_window_length; + } + } + else + { + _window_offset %= _current_window_length; + } + + // updte timing + _next_pulse_time += number_of_cycles; +} + +void DigitalPhaseLockedLoop::add_pulse() +{ + // TODO: linear regression to adjust _current_window_length and _next_pulse_time + + + // therefore, there was a 1 in this window + _window_was_filled = true; + if(_delegate) _delegate->digital_phase_locked_loop_output_bit(1); + + // shift history one to the left, subtracting the outgoing pulse from + unsigned int *const _pulse_history_array = _pulse_history.get(); + unsigned int outgoing_pulse_time = _pulse_history_array[0]; + + for(size_t pulse = 1; pulse < _length_of_history; pulse++) + { + _pulse_history_array[pulse - 1] = _pulse_history_array[pulse] - outgoing_pulse_time; + } + _next_pulse_time -= outgoing_pulse_time; + _pulse_history_array[_length_of_history-1] = _next_pulse_time; +} diff --git a/Storage/Disk/DigitalPhaseLockedLoop.hpp b/Storage/Disk/DigitalPhaseLockedLoop.hpp index f8b3910c1..a8364631f 100644 --- a/Storage/Disk/DigitalPhaseLockedLoop.hpp +++ b/Storage/Disk/DigitalPhaseLockedLoop.hpp @@ -9,6 +9,8 @@ #ifndef DigitalPhaseLockedLoop_hpp #define DigitalPhaseLockedLoop_hpp +#include + namespace Storage { class DigitalPhaseLockedLoop { @@ -18,8 +20,9 @@ class DigitalPhaseLockedLoop { @param clocks_per_bit The expected number of cycles between each bit of input. @param tolerance The maximum tolerance for bit windows — extremes will be clocks_per_bit ± tolerance. + @param length_of_history The number of historic pulses to consider in locking to phase. */ - DigitalPhaseLockedLoop(unsigned int clocks_per_bit, unsigned int tolerance); + DigitalPhaseLockedLoop(unsigned int clocks_per_bit, unsigned int tolerance, unsigned int length_of_history); /*! Runs the loop, impliedly posting no pulses during that period. @@ -47,6 +50,17 @@ class DigitalPhaseLockedLoop { private: Delegate *_delegate; + + std::unique_ptr _pulse_history; + unsigned int _current_window_length; + unsigned int _length_of_history; + + unsigned int _next_pulse_time; + unsigned int _window_offset; + bool _window_was_filled; + + unsigned int _clocks_per_bit; + unsigned int _tolerance; }; }