from pydsp import Signal
import pydsp.misc as misc
import pydsp.Modulation as Modulation
import pydsp.Window as Window
import pydsp.Filter as Filter
import math
import random
from pydsp.misc import *
######################################
# Digital Phase-Locked loop based on a
# NCO (Numerically Controlled Oscillator) and a second-order
# loop filter.
#
# The phase-detector is built by simulating the NCO generating
# the target signal, and then calculating the phase difference
# between it and the PLL-NCO.
#
# See http://www.us.design-reuse.com/articles/article5187.html for
# more details.
#
def pllme(fs, target_freq, target_phase, lo_freq, lo_phase, samples):
fs = float(fs)
# Angle counter for the target
p0 = float(target_phase)
# Angle counter for our local oscillator
p1 = float(lo_phase)
f = float(lo_freq)
te =[]
tf = []
for i in range(0, samples):
## Simulate two oscilators,
# Fist one is our target
p0 += math.pi * 2 * (float(target_freq) / fs)
# Second is our PLL
p1 += math.pi * 2 * (f / fs)
# And calculate the phase difference between them
# to get our error signal
e = p0 - p1
## Filter the error using a second order IIR filter
## using a proportional and an integral path
# Frequency adjustment, (integral path)
f += 0.004*(fs/(math.pi*2)) * e
# Adjust the current phase a bit (proportional path)
p1 += 0.2*e
# Keep the values so we can plot them later.
te.append(e)
tf.append(f)
print "Statistics at END of simulation:"
print " Processed "+str(samples)+" samples of signal"
print "\tTarget freq: "+str(target_freq)
print "\tTarget phase: "+str(p0 % (math.pi*2))
print "\tPLL-Freq: "+str(f)
print "\tPLL-Phase: "+str(p1 % (math.pi*2))
plot(tf)
plot(te)
pllme(fs = 1000, # 1kHz sample freq
target_freq = 104.242, #Hz is the freq we want
target_phase = 3*math.pi/4, # Phase offset for target
lo_freq = 100, # Start with PLL at 100Hz
lo_phase = 0, # and phase 0
samples = 1000 # Process this many samples
)