1# Bug corrections for version 1.9
2import cmsisdsp as dsp
3import cmsisdsp.fixedpoint as f
4import numpy as np
5import math
6import colorama
7from colorama import init,Fore, Back, Style
8from numpy.testing import assert_allclose
9#import matplotlib.pyplot as plt
10from scipy import signal
11
12init()
13
14def printTitle(s):
15    print("\n" + Fore.GREEN + Style.BRIGHT +  s + Style.RESET_ALL)
16
17def printSubTitle(s):
18    print("\n" + Style.BRIGHT + s + Style.RESET_ALL)
19
20printTitle("Decimate")
21
22test_length_seconds = 0.1
23signal_frequency = 100
24sampling_freq = 8000
25nbSamples = int(test_length_seconds*sampling_freq)
26wave = np.sin(2*np.pi*signal_frequency*np.linspace(0,test_length_seconds,nbSamples))
27
28#plt.plot(wave)
29#plt.show()
30
31decimationFactor = 4
32
33numTaps = 9
34downsamplingFilter = signal.firwin(numTaps,1.0 / decimationFactor)
35block_size = 160
36assert(block_size % decimationFactor == 0)
37
38ds_state = np.zeros(block_size + len(downsamplingFilter)-1)
39decimator = dsp.arm_fir_decimate_instance_f32()
40
41status = dsp.arm_fir_decimate_init_f32(decimator,numTaps,decimationFactor, downsamplingFilter, ds_state)
42
43def processSignal(sig,dec,f):
44    result = []
45    for blockNb in range(len(sig) // block_size):
46        s = blockNb * block_size
47        e = s + block_size
48
49        r = f(dec,sig[s:e])
50        result.append(r)
51    output = np.hstack(result)
52    return(output)
53
54ref = processSignal(wave,decimator,dsp.arm_fir_decimate_f32)
55#plt.plot(ref)
56#plt.show()
57
58printSubTitle("Decimate Q31")
59waveQ31 = f.toQ31(wave)
60downsamplingFilterQ31 = f.toQ31(downsamplingFilter)
61stateQ31 = np.zeros(block_size + len(downsamplingFilter)-1)
62decimatorQ31 = dsp.arm_fir_decimate_instance_q31()
63status = dsp.arm_fir_decimate_init_q31(decimatorQ31,numTaps,decimationFactor,
64         downsamplingFilterQ31, stateQ31)
65
66outputQ31 = processSignal(waveQ31,decimatorQ31,dsp.arm_fir_decimate_q31)
67outputF32 = f.Q31toF32(outputQ31)
68
69printSubTitle("Decimate Fast Q31")
70waveQ31 = f.toQ31(wave)
71downsamplingFilterQ31 = f.toQ31(downsamplingFilter)
72stateQ31 = np.zeros(block_size + len(downsamplingFilter)-1)
73decimatorQ31 = dsp.arm_fir_decimate_instance_q31()
74status = dsp.arm_fir_decimate_init_q31(decimatorQ31,numTaps,decimationFactor,
75         downsamplingFilterQ31, stateQ31)
76
77outputQ31 = processSignal(waveQ31,decimatorQ31,dsp.arm_fir_decimate_fast_q31)
78outputF32 = f.Q31toF32(outputQ31)
79
80printSubTitle("Decimate Q15")
81
82waveQ15 = f.toQ15(wave)
83downsamplingFilterQ15 = f.toQ15(downsamplingFilter)
84stateQ15 = np.zeros(block_size + len(downsamplingFilter)-1)
85decimatorQ15 = dsp.arm_fir_decimate_instance_q15()
86status = dsp.arm_fir_decimate_init_q15(decimatorQ15,numTaps,decimationFactor,
87         downsamplingFilterQ15, stateQ15)
88
89outputQ15 = processSignal(waveQ15,decimatorQ15,dsp.arm_fir_decimate_q15)
90outputF32 = f.Q15toF32(outputQ15)
91#plt.plot(outputF32)
92#plt.show()
93assert_allclose(ref,outputF32,rtol=2e-3,atol=1e-3)
94
95printSubTitle("Decimate Fast Q15")
96
97waveQ15 = f.toQ15(wave)
98downsamplingFilterQ15 = f.toQ15(downsamplingFilter)
99stateQ15 = np.zeros(block_size + len(downsamplingFilter)-1)
100decimatorQ15 = dsp.arm_fir_decimate_instance_q15()
101status = dsp.arm_fir_decimate_init_q15(decimatorQ15,numTaps,decimationFactor,
102         downsamplingFilterQ15, stateQ15)
103
104outputQ15 = processSignal(waveQ15,decimatorQ15,dsp.arm_fir_decimate_fast_q15)
105outputF32 = f.Q15toF32(outputQ15)
106#plt.plot(outputF32)
107#plt.show()
108assert_allclose(ref,outputF32,rtol=2e-3,atol=1e-3)
109
110
111printTitle("Interpolate")
112
113upsamplingFactor = 4
114
115numTaps = 16
116upsamplingFilter = signal.firwin(numTaps,1.0 / upsamplingFactor)
117assert(numTaps % upsamplingFactor == 0)
118block_size = 40
119
120
121printSubTitle("Interpolate F32")
122
123state = np.zeros(block_size + len(upsamplingFilter)//upsamplingFactor-1)
124interpolator = dsp.arm_fir_interpolate_instance_f32()
125status = dsp.arm_fir_interpolate_init_f32(interpolator,upsamplingFactor,numTaps,
126         upsamplingFilter, state)
127
128output = processSignal(ref,interpolator,dsp.arm_fir_interpolate_f32)
129output = output / np.max(output)
130#t = range(nbSamples)
131#plt.plot(t,wave,t[:-11],output[11:])
132#plt.show()
133
134d = 11
135assert_allclose(wave[:-d],output[d:],atol=0.1)
136
137printSubTitle("Interpolate Q31")
138
139upsamplingFilterQ31 = f.toQ31(upsamplingFilter)
140stateQ31 = np.zeros(block_size + len(upsamplingFilter)//upsamplingFactor-1)
141interpolatorQ31 = dsp.arm_fir_interpolate_instance_q31()
142status = dsp.arm_fir_interpolate_init_q31(interpolatorQ31,upsamplingFactor,numTaps,
143         upsamplingFilterQ31, stateQ31)
144
145outputQ31 = processSignal(outputQ31,interpolatorQ31,dsp.arm_fir_interpolate_q31)
146outputF32 = f.Q31toF32(outputQ31)
147outputF32 = outputF32 / np.max(outputF32)
148
149assert_allclose(wave[:-d],outputF32[d:],atol=0.1)
150
151printSubTitle("Interpolate Q15")
152
153upsamplingFilterQ15 = f.toQ15(upsamplingFilter)
154stateQ15 = np.zeros(block_size + len(upsamplingFilter)//upsamplingFactor-1)
155interpolatorQ15 = dsp.arm_fir_interpolate_instance_q15()
156status = dsp.arm_fir_interpolate_init_q15(interpolatorQ15,upsamplingFactor,numTaps,
157         upsamplingFilterQ15, stateQ15)
158
159outputQ15 = processSignal(outputQ15,interpolatorQ15,dsp.arm_fir_interpolate_q15)
160outputF32 = f.Q15toF32(outputQ15)
161outputF32 = outputF32 / np.max(outputF32)
162
163assert_allclose(wave[:-d],outputF32[d:],atol=0.1)
164