dpll.py

# Copyright (c) Mariusz Ryndzionek 2025
# License: MIT

import math
import cmath
import numpy as np

import matplotlib.pyplot as plt
import scipy.signal as sig


def pll_2nd_order_des(loop_bw, zeta=1 / math.sqrt(2), sr=math.pi):
    if zeta < 0 or zeta > 0.9:
        raise ValueError("zeta needs to be in the range [0.0, 0.9]")

    # prewarping
    wa = (2 * sr) * math.tan(2 * math.pi * loop_bw / (2 * sr))
    wn = wa / sr
    wn = 2 * math.pi * (loop_bw / sr)
    K = 1
    t1 = K / (wn * wn)
    t2 = 2 * zeta / wn

    b = [(2 * t2 + 1) / (2 * t1), (1 - 2 * t2) / (2 * t1)]
    a = [1, -1]

    return b, a


def pll_3rd_order_des(
    loop_bw, zeta=1 / math.sqrt(2), sr=math.pi, alternative_design=False
):
    if zeta < 0 or zeta > 0.9:
        raise ValueError("zeta needs to be in the range [0.0, 0.9]")

    # prewarping
    wa = (2 * sr) * math.tan(2 * math.pi * loop_bw / (2 * sr))
    wn = wa / sr
    wn = 2 * math.pi * (loop_bw / sr)
    if not alternative_design:
        b = 1 + 2 * zeta
        c = b
    else:
        cfg = {
            0.0: (0.3333, 0.5774),
            0.1: (0.6865, 0.589),
            0.2: (1.0269, 0.602),
            0.3: (1.3533, 0.6166),
            0.4: (1.6643, 0.6333),
            0.5: (1.9581, 0.6527),
            0.6: (2.2322, 0.6759),
            0.7: (2.4831, 0.7048),
            0.8: (2.7053, 0.7431),
            0.9: (3.1927, 1.3711),
        }
        b = 2.9999
        zetas = np.array(list(cfg.keys()))
        idx = np.argmin(np.abs(zetas - zeta))
        c, alpha = list(cfg.values())[idx]
        wn *= alpha

    bf = (
        b * wn**2 / 2 + c * wn + wn**3 / 4,
        -2 * c * wn + wn**3 / 2,
        -b * wn**2 / 2 + c * wn + wn**3 / 4,
    )
    af = [1, -2, 1]

    return bf, af


class DPLL:

    def __init__(self, loop_bw, sr, zeta=0.9, order=3, alternative_design=False):
        if order not in [2, 3]:
            raise ValueError("PLL order needs to be 2, or 3")

        if order == 2 and alternative_design:
            print("Warning: Alternative design only available for PLL order 3")

        if order == 2:
            self.bf, self.af = pll_2nd_order_des(loop_bw, zeta, sr)
        else:
            self.bf, self.af = pll_3rd_order_des(loop_bw, zeta, sr, alternative_design)
        print(f"PLL order: {order}")
        print(f"zeta: {zeta}")
        if alternative_design:
            print(f"Alternative design")
        print(f"Loop filter: b={self.bf}, a={self.af}")

        self.num_taps = len(self.bf)

        if len(self.bf) < 3:
            self.bf += [0] * (3 - len(self.bf))

        if len(self.af) < 3:
            self.af += [0] * (3 - len(self.af))

        self.phi_locked = 0.0
        self.y1 = 0
        self.x1 = 0
        self.y2 = 0
        self.x2 = 0

    def process(self, i, q):
        out = complex(math.cos(self.phi_locked), math.sin(self.phi_locked))
        err = cmath.phase(complex(i, q) * complex(out.real, -out.imag))

        # this is simplified for clarity
        # (as a reference for implementation in different languages)
        y0 = (
            err * self.bf[0]
            + self.x1 * self.bf[1]
            + self.x2 * self.bf[2]
            - self.y1 * self.af[1]
            - self.y2 * self.af[2]
        )
        self.y2 = self.y1
        self.y1 = y0
        self.x2 = self.x1
        self.x1 = err
        self.phi_locked += y0

        self.phi_locked = (
            (self.phi_locked > 2 * math.pi)
            and (self.phi_locked - 2 * math.pi)
            or self.phi_locked
        )
        self.phi_locked = (
            (self.phi_locked < -2 * math.pi)
            and (self.phi_locked + 2 * math.pi)
            or self.phi_locked
        )

        return out.real, out.imag, err


SR = 15000
TS = 1 / SR
et = 0.5
loop_bw = 30

time = np.arange(0, et, 1 / SR)

# add jumps to frequency
freq = np.linspace(100, 1000, time.size)
freq += (time > (et / 4)) * 150
freq -= (time > (2 * et / 4)) * 150
freq += (time > (3 * et / 4)) * 150

carrier = 0.5 * np.exp(2j * np.pi * freq * time)
# AM modulate
input = carrier * ((np.sin(2 * np.pi * -10 * time) + 1.2) / 2)
phase = np.angle(input)

pll = DPLL(loop_bw, SR)

output = []
error = []
for s in input:
    out_i, out_q, err = pll.process(np.real(s), np.imag(s))
    output.append(complex(out_i, out_q))
    error.append(err)

error = np.array(error)
output = np.array(output)
input_r = np.real(input)

plt.plot(time, np.real(output), label="PLL order 3 output")
plt.plot(time, input_r, label="Input signal")
plt.plot(time, error / np.pi, label="Phase error (divided by pi)")
plt.xlim(0, et)
plt.grid(True)
plt.legend()
plt.show()

pll = DPLL(loop_bw, SR, alternative_design=True)

error_alt = []
for s in input:
    out_i, out_q, err = pll.process(np.real(s), np.imag(s))
    error_alt.append(err)

error_alt = np.array(error_alt)

pll = DPLL(loop_bw, SR, order=2)

error_2 = []
for s in input:
    out_i, out_q, err = pll.process(np.real(s), np.imag(s))
    error_2.append(err)

error_2 = np.array(error_2)

plt.plot(time, error_2 / np.pi, label="Phase error (order 2)")
plt.plot(time, error / np.pi, label="Phase error (order 3)")
plt.plot(time, error_alt / np.pi, label="Phase error (order 3, alternative design")
plt.xlim(0, et)
plt.grid(True)
plt.legend()
plt.show()