Software Phase Lock loops

Status
Not open for further replies.
Yes, I have had it running in several forms, and am just in the process of converting to an audio block PLL for real time-processing. However, I'm focussing on the T3.6 these days and haven't tried it on a T3.2. My current versions use a lot of my own library functions, and are not quite ready for general distribution.

I did the original design and testing in MATLAB, then migrated that code to the T3.6 using floating point code for processing. I then set about to migrate it to ints to speed up the internal filters, etc. and looked at simplified algorithms, etc.

I could be convinced to tidy things up and make it available :D, what's your application?

Derek
 
Finally. I thought nobody haven't invented such a library. I'm quite lucky then. :D
I am just in the process of converting to an audio block PLL for real time-processing

Sounds interesting. If you have some time please try it with Teensy 3.2 as well. I know its not an easy task since you have written that for floating point. But again you need some time to convert back it into fixed point.

I'm using PLL for lock on certain frequencies. Fundamental and some overtones.
 
Unfortunately I don't have a functional T3.2 anymore!

I am working on the PLL for the same purpose as you, but specifically for electronic music analysis/synthesis. I am, however, spread extremely thin right now with several other projects - all demanding my full attention!

To keep the ball rolling, here is the code for the T3.6 self-contained Teensy PLL simulation - which relies heavily on floating point:
Code:
//---------------------------------------------------------------------------------------
//     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;
    }
  }
}
I'll "try" to organize and post the int16 version later today, so you can try it on the T3.2 (but don't hold your breath :D ).

- Derek
 
Great work. Ill check with this now. Anyway if you find out some time organize and post the int16 version.
 
I have the int16 version ready to post - except for one little "bug" that I hadn't noticed before: when it locks to a sinusoid the reported phase error is a constant (not zero) that is proportional to the frequency! I can't understand it - the code looks to be identical to what you have now. I cant see whether it's the printout or actually there. A phase that's proportional to frequency is generally indicative of a time delay...

I have one more task I want to complete today: the posting of a Teensy audio block for an "adaptive-least-squares" audio filter for interference rejection or narrow-band signal enhancement (specifically for use in software-defined-radio (SDR) - which is another on-going project of mine). Then I'll get back to the PLL stuff...
 
Did you able to figure out the issue? If not post it. I have checked you'r code with Teensy 3.2 as well. Seems like it working. Whats the different between int16 version and previously posted code?
 
Derek

Is that possible to detect certain frequencies from adc input signals(Microphone signals) ?
Something like if i want to lock frequency 450Hz and need a capture range of 400Hz-500Hz something like that? Did you able to make audio block ?
 
@Jacob2,
No, it's not quite ready yet. For both you and Wicky21, I'll PM you both about what is going on with me and I'm sure you'll understand.
The problem is in the integer version of the fast (approximate) phase detector I developed, it's not unwrapping the phase properly... I have a policy of not releasing code before I've properly tested it. Please be patient.

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).



@Wicky21,
I'll post back on the frequency estimation thread, butThe PLL was to be my next suggestion for your project. Now that I understand what you are trying to do, I'm not so sure that the FFT methods are really so suitable. The problem with the PLL would be the capture time before the signal is locked.
 
@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).

No hurry at all Derek. Interested about "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".
As Wicky21 said im also using some external microphone to capture real time signals. Then only using Software PLL for processing
 
Good news! Yesterday I finally got the int16 version debugged and working, and it works beautifully - just as well as my MATLAB simulation. There's just one little "teensy""problem: every 10 secs or so there is a very short (2-3 samples) "glitch"in the output. Quite random. I must be overloading a register somewhere, and may need to add another guard bit.

I'll be starting on the audio block today, and it should be just a matter of copy and paste. I have a couple of questions about what outputs to provide to make it generally useful:
1) I propose to stream out the oscillator output as a clean sinusoid.
2) I'm also thinking about outputting the oscillator frequency in Hz also on a second channel.
Would both be useful for real-time use? Anything else?

I'm also thinking about function calls. How about for starters:
PLL.enable()
PLL.disable()
PLL.passthrough()
PLL.frequency() -- returns the current frequency in Hz
PLL.isLocked() -- boolean to indicate a valid lock
PLL.filterParameters() -- new values of w_n, zeta, and Ka for the loop filter design
Anything else?
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.
 
Last edited:
Great news Derek. But i'm not quite sure about general usage.

Code:
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
That's going to be useful.
Anyway when you'r releasing block if possible release MATLAB code for PLL as well. It's going to be useful.
 
Here's the MATLAB code as requested
Code:
%--------------------------------------------------------------------------
%  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;
Enjoy.
 
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
 
Great work.
Derek I don't understand deeply what you have explained. Some how I guess software PLL can be use with microphone input signal with good sensitivity right.
I never had used software PLLs. Instead I have used LM567 Analog PLL. In there we change RC and adjust center freq as well as bandwidths. But it can only adjust one center freq with limited bandwidth with one IC. Hope software PLL can overcome all this issues. I have explain you about how my prj can involve with PLL.
Anyway when you release the PLL, just put some instructions how properly use the each functions.
Good Luck!!!
 
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

Hi Derek, Very interested in your Teensy PLL project, I am requiring for my project a PLL with a range between 1Khz-20Khz, would this be possible using your code?
Many Thanks
 
Status
Not open for further replies.
Back
Top