Does anybody have a written software phase lock loop library for teensy 3.2???
Does anybody have a written software phase lock loop library for teensy 3.2???
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, what's your application?
Derek
Finally. I thought nobody haven't invented such a library. I'm quite lucky then.
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 am just in the process of converting to an audio block PLL for real time-processing
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:
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 breathCode://--------------------------------------------------------------------------------------- // 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; } } }
- 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.
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".Originally Posted by DerekR
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 by DerekR; 09-16-2017 at 12:05 PM.
Great news Derek. But i'm not quite sure about general usage.
That's going to be useful.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
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
Enjoy.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;
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!!!
I have just posted the AudioPLL stuff in its own thread here
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
Posting Permissions
Reply With Quote