I am just in the process of converting to an audio block PLL for real time-processing
//---------------------------------------------------------------------------------------
// File: TestPLL.ino
// Purpose: Evaluate Phase-Locked-Loop (PLL) operation on the Teesy 3.6
//
// Description: The code accepts an input frequency from the keyboard, generates
// a sinusoid at that frequency and uses the PLL to estimate that frequency,
// Estimates of the frequency and phase error are printed at each loop iteration.
//
// Notes: 1) This code is for evaluation purposes only. It primarily used
// as a development tool, and is not intended for practical use.
// 2) This code makes heavy use of floating-point processing.
// 3) An int16 version has also been developed, and a Teensy
// "Audio" block version for real-time operation is under develoment.
//
// Author: Derek Rowell
// Updated: Sept. 7, 2017
//---------------------------------------------------------------------------------------
//
const int16_t SineTable[321] = {
0, 804, 1608, 2410, 3212, 4011, 4808, 5602, 6393, 7179,
7962, 8739, 9512, 10278, 11039, 11793, 12539, 13279, 14010, 14732,
15446, 16151, 16846, 17530, 18204, 18868, 19519, 20159, 20787, 21403,
22005, 22594, 23170, 23731, 24279, 24811, 25329, 25832, 26319, 26790,
27245, 27683, 28105, 28510, 28898, 29268, 29621, 29956, 30273, 30571,
30852, 31113, 31356, 31580, 31785, 31971, 32137, 32285, 32412, 32521,
32609, 32678, 32728, 32757, 32767, 32757, 32728, 32678, 32609, 32521,
32412, 32285, 32137, 31971, 31785, 31580, 31356, 31113, 30852, 30571,
30273, 29956, 29621, 29268, 28898, 28510, 28105, 27683, 27245, 26790,
26319, 25832, 25329, 24811, 24279, 23731, 23170, 22594, 22005, 21403,
20787, 20159, 19519, 18868, 18204, 17530, 16846, 16151, 15446, 14732,
14010, 13279, 12539, 11793, 11039, 10278, 9512, 8739, 7962, 7179,
6393, 5602, 4808, 4011, 3212, 2410, 1608, 804, 0, -804,
-1608, -2410, -3212, -4011, -4808, -5602, -6393, -7179, -7962, -8739,
-9512,-10278,-11039,-11793,-12539,-13279,-14010,-14732,-15446,-16151,
-16846,-17530,-18204,-18868,-19519,-20159,-20787,-21403,-22005,-22594,
-23170,-23731,-24279,-24811,-25329,-25832,-26319,-26790,-27245,-27683,
-28105,-28510,-28898,-29268,-29621,-29956,-30273,-30571,-30852,-31113,
-31356,-31580,-31785,-31971,-32137,-32285,-32412,-32521,-32609,-32678,
-32728,-32757,-32767,-32757,-32728,-32678,-32609,-32521,-32412,-32285,
-32137,-31971,-31785,-31580,-31356,-31113,-30852,-30571,-30273,-29956,
-29621,-29268,-28898,-28510,-28105,-27683,-27245,-26790,-26319,-25832,
-25329,-24811,-24279,-23731,-23170,-22594,-22005,-21403,-20787,-20159,
-19519,-18868,-18204,-17530,-16846,-16151,-15446,-14732,-14010,-13279,
-12539,-11793,-11039,-10278, -9512, -8739, -7962, -7179, -6393, -5602,
-4808, -4011, -3212, -2410, -1608, -804, 0, 804, 1608, 2410,
3212, 4011, 4808, 5602, 6393, 7179, 7962, 8739, 9512, 10278,
11039, 11793, 12539, 13279, 14010, 14732, 15446, 16151, 16846, 17530,
18204, 18868, 19519, 20159, 20787, 21403, 22005, 22594, 23170, 23731,
24279, 24811, 25329, 25832, 26319, 26790, 27245, 27683, 28105, 28510,
28898, 29268, 29621, 29956, 30273, 30571, 30852, 31113, 31356, 31580,
31785, 31971, 32137, 32285, 32412, 32521, 32609, 32678, 32728, 32757,
32767
};
//
float a1, a2;
float tau_1, tau_2;
float b0, b1, b2;
float inputPhaseInc;
float buff_0, buff_1, buff_2; // filter buffers
float phi; // input signal's initial phase
uint16_t RunLength = 200; // number of samples in simulation run
float pi = 3.141592653;
float phase_offset = 0.00f; // carrier phase offset
float frequency_offset = 0.20f; // carrier frequency offset
float wn = 0.03f; // pll bandwidth
float zeta = 0.707f; // pll damping factor
float Ka = 1000; // pll loop gain
float phi_hat = 0.0f; // PLL's output phase
float old_phi_hat = 0.0f;
float freq_hat = 0.0f; // PLL's output phase
float old_freq_hat = 0.0f;
double Fsample = 44100.0;
double smoothed_freq = 0.0f;
double old_smoothed_freq = 0.0f;
//---------------------------------------------------------------------------------
void setup() {
Serial.begin(9600); while (!Serial) {;}
// Design the bi-quad IIR loop filter
// Loop filter is:
// (1 + tau_2.s)
// G(s) = -------------
// tau_1.s^2
tau_1 = Ka/(wn*wn);
tau_2 = 2*zeta/wn;
// Digital filter coefficients found from bilinear tramsform:
b0 = (2*Ka/tau_1)*(1.0 + 2.0*tau_2);
b1 = (2*Ka/tau_1)*(1.0 - 2.0*tau_2);
b2 = 0.0;
a1 = -1.0f;
a2 = 0.0f;
Serial.println("Loop filter design parameters:");
Serial.print(" b = "); Serial.print(b0, 4); Serial.print(", ");
Serial.print(b1, 4); Serial.print(", "); Serial.println(b2, 4);;
Serial.print(" a = "); Serial.print(1.0, 4); Serial.print(", ");
Serial.print(a1, 4); Serial.print(", "); Serial.println(a2, 4);
Serial.println();
// Initialize the filter buffer (delay register)
buff_0 = 0.0f;
buff_1 = 0.0f;
buff_2 = 0.0f;
// Initialize states
phi = phase_offset; // input signal's initial phase
phi_hat = 0.0; // PLL's initial estimated phase
old_phi_hat = 0.0;
freq_hat = 0.0;
old_freq_hat = 0.0;
}
//---------------------------------------------------------------------------------
void loop(){
double frequency = getDouble("Enter frequency: ");
inputPhaseInc = 2*pi*frequency/Fsample; // Fetch input frequency
RunLength = getInt16("Number of loops (suggest >500): "); 678 // Fetch number of loops in this simulation
Serial.println();
simulate();
}
//---------------------------------------------------------------------------------
void simulate(){
float xReal, xImag;
float yReal, yImag;
float num;
float denom;
float phaseError = 0.0;
float deltaPhase;
float deltaReal;
float deltaImag;
int16_t realSign;
int16_t imagSign;
//---
// Start of PLL computational loop:
for (uint16_t i=0; i<RunLength; i++) {
// Compute input sinusoid and update phase
xReal = cos(phi);
xImag = sin(phi);
phi += inputPhaseInc; // update phase accumulator
if (phi > 2*pi) phi -= 2*pi;
//--
// Feedback from VCO phase: phi_hat
yReal = cos(phi_hat);
yImag = sin(phi_hat);
//--
// Phase detector - multply quadrature components
deltaReal = (xReal*yReal + xImag*yImag);
deltaImag = (xImag*yReal - xReal*yImag);
// Simple atan2 estimator - first-order rational approximation
// Note: We don't need accurate phase because all we are doing
// is creating a feedback error signal that will be driven
// to zero.
// Convert phase to the first quadrant;
realSign = 1;
imagSign = 1;
if (deltaReal<0){
deltaReal = -deltaReal;
realSign = -1;
}
if (deltaImag<0){
deltaImag = -deltaImag;
imagSign = -1;
}
// Calculate approximate phase error
num = deltaImag*(pi/2);
denom = deltaReal + deltaImag;
deltaPhase = num/denom;
// Restore the quadrant information:
if (realSign<0) deltaPhase = pi - deltaPhase;
if (imagSign<0) deltaPhase = -deltaPhase;
phaseError = deltaPhase;
//
//--
// Loop filter: Direct Form II, biquad second-order IIR:
buff_2 = buff_1; // shift center register to upper register
buff_1 = buff_0; // shift lower register to center register
buff_0 = phaseError - buff_1*a1 - buff_2*a2;
freq_hat = buff_0*b0 + buff_1*b1 + buff_2*b2;
// Trapezoidal integration to estimate phase:
phi_hat = old_phi_hat + 0.5*(freq_hat + old_freq_hat);
if (phi_hat > pi) phi_hat = phi_hat - 2*pi;
old_freq_hat = freq_hat;
old_phi_hat = phi_hat;
// We are all done: end of PLL computational loop...
Serial.print(i+1);
Serial.print("\tEstimated Freq: "); Serial.print(freq_hat*Fsample/(2.*pi), 4);
Serial.print("\t Phase Error: "); Serial.print(phaseError*180./pi, 4); Serial.println(" deg.");
}
}
//---------------------------------------------------------------------------------
// Simple function to input a float from the Serial input:
double getDouble(const char * prompt) {
double val;
String inPrompt = prompt;
String inString = "";
Serial.print(inPrompt);
while(1) {
while (!Serial.available()) { }
int inChar = Serial.read();
if (inChar != '\n') {
inString += (char)inChar;
}
else {
val = int16_t(inString.toFloat());
Serial.println(val);
return val;
}
}
}
//---------------------------------------------------------------------------------
// Simple function to input an int16_t from the Serial input:
int16_t getInt16(const char * prompt) {
int16_t val;
String inPrompt = prompt;
String inString = "";
Serial.print(inPrompt);
while(1) {
while (!Serial.available()) { }
int inChar = Serial.read();
if (inChar != '\n') {
inString += (char)inChar;
}
else {
val = int16_t(inString.toInt());
Serial.println(val);
return val;
}
}
}
@Jacob2,
The other problem I've been addressing is that the present version was designed to work with quadrature (complex) input signals in a medical ultrasound project. I need to modify it to work with real inputs - and that needs a new phase detector, of the inclusion of a Hilbert transformer to produce the quadrature signal. (I already have the Teensy code for that).
I'll start with the quadrature input version, then investigate the use of my Hilbert transform block for real inputs, and finally include it directly
%--------------------------------------------------------------------------
% Prototype for Teensy 3.x Phased-locked-loop (PLL) - simulation
% Derek Rowell
% Sept 16, 2017
%--------------------------------------------------------------------------
%
% Design of the bi-quad IIR loop filter;
% Choose the loop filter as:
% Ka(1 + tau_2*s)
% G(s) = ----------------
% tau_1*s^2
% Note that this is a Type 2 controller, ie it has a pair of "free" integrators, and will
% reduce steady-state phase and frequency errors to zero.
% Aside: we will implement the filter two cascaded sections
% Ka(1 + tau_2*s) Ka(1 + tau_2*s) 1
% G(s) = ---------------- = ---------------- . --- = G1(s).G2(s)
% tau_1*s^2 tau_1*s s
% so that we can compute both the frequency (from the first section), and the phase (from the second)
%
% The closed-loop transfer function is
%
% G(s) Ka(1+tau2*s) wn^2*(1+tau2*s)
% G_cl(s) = -------- = ------------------------- = -------------------------
% 1 + G(s) tau_1*s^2 +Ka*tau_2*s +Ka s^2 + 2*zeta*wn*s + wn^2
% where
% wn^2 = Ka/tau_1 or tau_1 = Ka/wn^2
% Ka*tau_2/tau_1 = 2*zeta*wn or tau_2 = 2*zeta/wn
% (We will use the standard second=order parameters wn, zeta, and Ka as our design inputs,
% since they have physical meaning to most enginers)
%
% The digital filter H(z) is designed using the bilinear tranform
% 2 (z-1)
% G(z) = G(s) with s = - ----- substituted, and let T=1
% T (z+1)
% b_0*z^(0) + b_1*z^(-1) (z+1) (1 + z^(-1))
% G_1(z) = -------------------- , and for the intregrator G_2(z) = ------ = -------------
% a_0*z^(0) + a_1*z^(-1) 2(z-1) 2(1-z^^(-1))
% with
% b_0 = (2*Ka/tau_1)*(1.0 + 2.0*tau_2), b1 =(2*Ka/tau_1)*(1.0 - 2.0*tau_2);
% a_0 = 1, a_1 = -1
%
% Then the computational difference equations are:
% freq(n+1) = -a_1*freq(n) + (b_0*phase_error(n) + b_1*phase_error(n-1)) for G_1(z),
% phase(n+1) = phase(n) + (freq(n) + freq(n-1))/2 which is the trapezoidal (Tustins) integrator
%--------------------------------------------------------------------------------------
%
clear all;
% Run parameters:
Input_frequency = 300;
Initial_phase_offset = 0.0;
Run_length = 1000; % number of samples in run
Fsample = 44100; % CD sampling frequency (for Teensy)
%
w_n = 0.03; % PLL closed-loop natural frequency
zeta = 0.707; % PLL closed-loop damping ratio
Ka = 1000; % PLL loop gain
%--------------------------------------------------------------------------
%
% Loop filter design:
% Filter is 2nd-order IIR bi-quad
tau_1 = Ka/(w_n*w_n);
tau_2 = 2*zeta/w_n;
% Feed-forward coefficients (numerator)
b_0 = (2*Ka/tau_1)*(1.0 + 2.0*tau_2);
b_1 = (2*Ka/tau_1)*(1.0 - 2.0*tau_2);
% Feed-back coefficients (denominator)
% a_0 = 1.0 is implied
a_1 = -1.0;
% Initialize the filter delay regiser
delay_0 = 0.0;
delay_1 = 0.0;
delay_2 = 0.0;
% initialize states:
phi = Initial_phase_offset; % input signal's initial phase
phase_est = zeros(1,Run_length);
prev_phase_est = 0.0;
prev_freq_est = 0.0;
freq_est = zeros(1,Run_length);
phase = 0;
phase_inc = 2*pi*Input_frequency/Fsample; % phase step for input signal
%---------------------------------------------------------
% Simulation:
for i=1:Run_length
% Compute input sinusoid, and update phase for the next cycle
x(i) = complex(cos(phi),sin(phi));
phi = phi + phase_inc;
if (phi > 2*pi)
phi = phi - 2*pi;
end
% Compute PLL local oscillator output from phase estimate
y(i) = complex(cos(phase), sin(phase));
Phase_error(i) = angle(x(i)*conj(y(i)));
%-------------------------------------------------------
% Direct Form II IIR filter
% Filter the error through loop filter, updating phase estimate
% Shift register:
delay_1 = delay_0;
delay_0 = Phase_error(i) - delay_1*a_1;
% Compute new frequency estimate for next loop:
freq_est(i) = delay_0*b_0 + delay_1*b_1;
% Trapezoidal integration to estimate phase
phase_est(i) = prev_phase_est + 0.5*(freq_est(i) + prev_freq_est);
if (phase_est(i) > 2*pi)
phase_est(i) = phase_est(i) - 2*pi;
end
prev_freq_est = freq_est(i);
prev_phase_est = phase_est(i);
phase = phase_est(i);
end
xval = (1:Run_length)* 1000/Fsample;
%
figure(1)
plot(xval,real(x),xval,real(y));
title('PLL - Input/Output Waveforms');
xlabel('Time (msecs)');
ylabel('Real components of input and output');
grid on;
%
figure(2)
plot(xval,Phase_error);
title('PLL - Phase Error');
xlabel('Time (msecs)');
ylabel('Phase Error (radians)');
grid on;
%
figure(3)
plot(xval,freq_est*2*pi/180);
title('PLL - Frequency Error');
xlabel('Time (msecs)');
ylabel('Frequency Error (Hz)');
grid on;
Latest update:
1) The quadrature input Audio Board version has been up-and-running all weekend. It's ready to go.
2) The single-channel input version, which should be more useful to everybody, has been giving me all sorts of problems with the Hilbert Transform code. I certainly knew that all software transformers, both filters and FFT methods) are basically band-pass devices, however I hadn't appreciated how sensitive the PLL phase detector is to amplitude differences between the two input channels. (I really do want to retain the complex-arithmetic based phase detector - it's just the right thing to do for theoretical and practical reasons). Most Hilbert transformers use a delay-line for one quadrature channel, while the other passes through the band-pass filter, which means that at low frequencies (around 200Hz) I was getting very poor performance. I think I've taken the filter design packages as low as they can comfortably go It's now working to about 100Hz...
This afternoon I set out to build an automatic-level-control system that will keep the quadrature outputs matched in amplitude at even lower frequencies. It's looking very promising. It is certainly matching the levels below 50 Hz, but now I'm hitting problems with needing long averaging times. I'll be trying to integrate it in later, and if things look good I'll clean up the code and post it all tomorrow.
I'll post it in a new thread...
Derek