mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 09:45:33 +03:00
79 lines
No EOL
1.9 KiB
Python
79 lines
No EOL
1.9 KiB
Python
import cmsisdsp as dsp
|
|
import numpy as np
|
|
from scipy import signal
|
|
from pylab import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show,semilogx, semilogy
|
|
# Data file from https://www.physionet.org/pn3/ecgiddb/Person_87/rec_2.dat
|
|
|
|
def q31sat(x):
|
|
if x > 0x7FFFFFFF:
|
|
return(np.int32(0x7FFFFFFF))
|
|
elif x < -0x80000000:
|
|
return(np.int32(0x80000000))
|
|
else:
|
|
return(np.int32(x))
|
|
|
|
q31satV=np.vectorize(q31sat)
|
|
|
|
def toQ31(x):
|
|
return(q31satV(np.round(x * (1<<31))))
|
|
|
|
def Q31toF32(x):
|
|
return(1.0*x / 2**31)
|
|
|
|
filename = 'rec_2.dat'
|
|
|
|
f = open(filename,"r")
|
|
sig = np.fromfile(f, dtype=np.int16)
|
|
f.closed
|
|
|
|
sig = 1.0*sig / (1 << 12)
|
|
|
|
|
|
p0 = np.exp(1j*0.05) * 0.98
|
|
p1 = np.exp(1j*0.25) * 0.9
|
|
p2 = np.exp(1j*0.45) * 0.97
|
|
|
|
z0 = np.exp(1j*0.02)
|
|
z1 = np.exp(1j*0.65)
|
|
z2 = np.exp(1j*1.0)
|
|
|
|
g = 0.02
|
|
|
|
nb = 300
|
|
|
|
sos = signal.zpk2sos(
|
|
[z0,np.conj(z0),z1,np.conj(z1),z2,np.conj(z2)]
|
|
,[p0, np.conj(p0),p1, np.conj(p1),p2, np.conj(p2)]
|
|
,g)
|
|
|
|
res=signal.sosfilt(sos,sig)
|
|
figure()
|
|
plot(sig[1:nb])
|
|
figure()
|
|
plot(res[1:nb])
|
|
|
|
|
|
|
|
|
|
biquadQ31 = dsp.arm_biquad_casd_df1_inst_q31()
|
|
numStages=3
|
|
state=np.zeros(numStages*4)
|
|
# For use in CMSIS, denominator coefs must be negated
|
|
# and first a0 coef wihich is always 1 must be removed
|
|
coefs=np.reshape(np.hstack((sos[:,:3],-sos[:,4:])),15)
|
|
coefs = coefs / 4.0
|
|
coefsQ31 = toQ31(coefs)
|
|
postshift = 2
|
|
dsp.arm_biquad_cascade_df1_init_q31(biquadQ31,numStages,coefsQ31,state,postshift)
|
|
sigQ31=toQ31(sig)
|
|
nbSamples=sigQ31.shape[0]
|
|
# Here we demonstrate how we can process a long sequence of samples per block
|
|
# and thus check that the state of the biquad is well updated and preserved
|
|
# between the calls.
|
|
half = int(round(nbSamples / 2))
|
|
res2a=dsp.arm_biquad_cascade_df1_q31(biquadQ31,sigQ31[1:half])
|
|
res2b=dsp.arm_biquad_cascade_df1_q31(biquadQ31,sigQ31[half+1:nbSamples])
|
|
res2=Q31toF32(np.hstack((res2a,res2b)))
|
|
figure()
|
|
plot(res2[1:nb])
|
|
show()# |