1import cmsisdsp as dsp
2import numpy as np
3from scipy import signal
4from pylab import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show,semilogx, semilogy
5# Data file from https://archive.physionet.org/pn3/ecgiddb/Person_87/rec_2.dat
6
7def q31sat(x):
8     if x > 0x7FFFFFFF:
9          return(np.int32(0x7FFFFFFF))
10     elif x < -0x80000000:
11          return(np.int32(0x80000000))
12     else:
13          return(np.int32(x))
14
15q31satV=np.vectorize(q31sat)
16
17def toQ31(x):
18     return(q31satV(np.round(x * (1<<31))))
19
20def Q31toF32(x):
21     return(1.0*x / 2**31)
22
23filename = 'rec_2.dat'
24
25f = open(filename,"r")
26sig = np.fromfile(f, dtype=np.int16)
27f.closed
28
29sig = 1.0*sig / (1 << 12)
30
31
32p0 = np.exp(1j*0.05) * 0.98
33p1 = np.exp(1j*0.25) * 0.9
34p2 = np.exp(1j*0.45) * 0.97
35
36z0 = np.exp(1j*0.02)
37z1 = np.exp(1j*0.65)
38z2 = np.exp(1j*1.0)
39
40g = 0.02
41
42nb = 300
43
44sos = signal.zpk2sos(
45      [z0,np.conj(z0),z1,np.conj(z1),z2,np.conj(z2)]
46     ,[p0, np.conj(p0),p1, np.conj(p1),p2, np.conj(p2)]
47     ,g)
48
49res=signal.sosfilt(sos,sig)
50figure()
51plot(sig[1:nb])
52figure()
53plot(res[1:nb])
54
55
56
57
58biquadQ31 = dsp.arm_biquad_casd_df1_inst_q31()
59
60numStages=3
61state=np.zeros(numStages*4)
62# For use in CMSIS, denominator coefs must be negated
63# and first a0 coef wihich is always 1 must be removed
64coefs=np.reshape(np.hstack((sos[:,:3],-sos[:,4:])),15)
65coefs = coefs / 4.0
66coefsQ31 = toQ31(coefs)
67postshift = 2
68dsp.arm_biquad_cascade_df1_init_q31(biquadQ31,numStages,coefsQ31,state,postshift)
69sigQ31=toQ31(sig)
70nbSamples=sigQ31.shape[0]
71# Here we demonstrate how we can process a long sequence of samples per block
72# and thus check that the state of the biquad is well updated and preserved
73# between the calls.
74half = int(round(nbSamples / 2))
75res2a=dsp.arm_biquad_cascade_df1_q31(biquadQ31,sigQ31[1:half])
76res2b=dsp.arm_biquad_cascade_df1_q31(biquadQ31,sigQ31[half+1:nbSamples])
77res2=Q31toF32(np.hstack((res2a,res2b)))
78figure()
79plot(res2[1:nb])
80show()#