fmsim.py

import numpy as np
import matplotlib.pyplot as plt

from scipy import signal


def plt_graph(plt, xlabel, title, x, y, color='b'):
    plt.figure()
    plt.xlabel(xlabel)
    plt.title(title)
    plt.plot(x, y, color)

###############################


sample_rate = 1.0e6
carrier_freq = 20e3
signal_freq = 1e3
fn = sample_rate/2.0

duration = 1.0/signal_freq*3

xs = np.arange(0, duration, 1/sample_rate)
ts = xs * 1000

# signal
ys1 = np.sin(2.0*np.pi*signal_freq*xs)
plt_graph(plt, "time[ms]", "Signal", ts, ys1)

# carrier
carrier = np.sin(2.0*np.pi*carrier_freq*xs)
plt_graph(plt, "time[ms]", "Carrier Singal", ts, carrier)

# modulation
m = 5
# m = 1
ys2 = np.sin(2.0*np.pi*carrier_freq*xs + m*ys1)
plt_graph(plt, "time[ms]", "FM Singal", ts, ys2)

# LO generation
lo_i = np.cos(2.0*np.pi*carrier_freq*xs)
lo_q = np.sin(2.0*np.pi*carrier_freq*xs)
plt.figure()
plt.xlabel("time[ms]")
plt.title("LO Signal")
plt.plot(ts, lo_i, 'r')
plt.plot(ts, lo_q, 'g')

# mixing
bbi = ys2*lo_i
bbq = ys2*lo_q
plt.figure()
plt.xlabel("time[ms]")
plt.title("Mixed Signal")
plt.plot(ts, bbi, 'r')
plt.plot(ts, bbq, 'g')


# head = [0]*100

# bbi = np.concatenate((head, bbi))
# bbq = np.concatenate((head, bbq))

# LPF
fp = 1.1e3
# fs = 2e6
# gpass = 1
# gstop = 40

Wp = fp/fn
# Ws = fs/fn
fir = signal.firwin(127, Wp)
bbi2 = signal.lfilter(fir, 1, bbi)
bbq2 = signal.lfilter(fir, 1, bbq)

# bbi2 = bbi2[100::]
# bbq2 = bbq2[100::]

plt.figure()
plt.xlabel("time[ms]")
plt.title("LPF Signal")
plt.plot(ts, bbi2, 'r')
plt.plot(ts, bbq2, 'g')

th_q = list(map(lambda x: 1 if x > 0 else 0, bbq2))
th_i = list(map(lambda x: 1 if x > 0 else 0, bbi2))

# arctan
phi_sig = np.ndarray(0)
hoge = np.diff(th_q)
hoge = np.append(hoge, 0)
offset = 0
for index, (i, q) in enumerate(zip(bbi2, bbq2)):
    phi_sig = np.append(phi_sig, np.arctan2(q, i) + offset)

    if th_i[index] == 0 and index > 0:
        if hoge[index] > 0:
            offset = offset - np.pi * 2.0
        elif hoge[index] < 0:
            offset = offset + np.pi * 2.0


# offset = list(map(lambda i, q: phi_offset(i, q), bbi2, bbq2))
# hoge = np.diff(th_q)
# hoge = np.append(hoge, 0)*2
# phi_sig = np.arctan2(bbq2, bbi2) + offset


plt.figure()
plt.xlabel("time[ms]")
plt.title("phi")
plt.plot(ts, phi_sig, 'g')
# plt.plot(ts, th_q, 'r')
# plt.plot(ts, th_i, 'b')
# plt.plot(ts, hoge, 'black')

# diff
demod = np.diff(phi_sig)

start = int(len(xs)/3)
print(start)
end = start + int(len(xs)/3)
print(end)
# plt_graph(plt, "time[ms]", "Demodulated", ts[:len(demod)], demod)
plt_graph(plt, "time[ms]", "Demodulated", ts[start:end], demod[start:end])

plt.show()