1import cmsisdsp as dsp
2import numpy as np
3from scipy import signal
4from scipy.fftpack import dct
5import cmsisdsp.fixedpoint as f
6from pyquaternion import Quaternion
7
8import colorama
9from colorama import init,Fore, Back, Style
10import statsmodels.tsa.stattools
11
12import scipy.spatial
13
14
15init()
16
17def printTitle(s):
18    print("\n" + Fore.GREEN + Style.BRIGHT +  s + Style.RESET_ALL)
19
20def printSubTitle(s):
21    print("\n" + Style.BRIGHT + s + Style.RESET_ALL)
22
23
24def imToReal2D(a):
25    ar=np.zeros(np.array(a.shape) * [1,2])
26    ar[::,0::2]=a.real
27    ar[::,1::2]=a.imag
28    return(ar)
29
30def realToIm2D(ar):
31    return(ar[::,0::2] + 1j * ar[::,1::2])
32
33def normalize(a):
34  return(a/np.max(np.abs(a)))
35
36def autocorr(x):
37    result = np.correlate(x, x, mode='full')
38    return result[result.size//2:]
39
40#################### MAX AND ABSMAX ##################################
41printTitle("Max and AbsMax")
42a=np.array([1.,-3.,4.,0.,-10.,8.])
43
44printSubTitle("Float tests")
45i=dsp.arm_max_f32(a)
46print(i)
47
48i=dsp.arm_absmax_f32(a)
49print(i)
50
51printSubTitle("Fixed point tests")
52
53# Normalize for fixed point tests
54a = a / i[0]
55
56a31 = f.toQ31(a)
57i=dsp.arm_absmax_q31(a31)
58print(f.Q31toF32(i[0]),i[1])
59
60a15 = f.toQ15(a)
61i=dsp.arm_absmax_q15(a15)
62print(f.Q15toF32(i[0]),i[1])
63
64a7 = f.toQ7(a)
65i=dsp.arm_absmax_q7(a7)
66print(f.Q7toF32(i[0]),i[1])
67
68################### MIN AND ABSMIN ################################
69
70printTitle("Min and AbsMin")
71a=np.array([1.,-3.,4.,0.5,-10.,8.])
72
73printSubTitle("Float tests")
74i=dsp.arm_min_f32(a)
75print(i)
76
77i=dsp.arm_absmin_f32(a)
78print(i)
79
80printSubTitle("Fixed point tests")
81
82# Normalize for fixed point tests
83idx=i[1]
84i=dsp.arm_absmax_f32(a)
85a = a / i[0]
86print(a)
87print(a[idx])
88
89a31 = f.toQ31(a)
90i=dsp.arm_absmin_q31(a31)
91print(f.Q31toF32(i[0]),i[1])
92
93a8 = f.toQ15(a)
94i=dsp.arm_absmin_q15(a8)
95print(f.Q15toF32(i[0]),i[1])
96
97a7 = f.toQ7(a)
98i=dsp.arm_absmin_q7(a7)
99print(f.Q7toF32(i[0]),i[1])
100
101##################### CLIPPING ###################
102printTitle("Clipping tests tests")
103a=np.array([1.,-3.,4.,0.5,-10.,8.])
104i=dsp.arm_absmax_f32(a)
105
106minBound =-5.0
107maxBound =6.0
108b=dsp.arm_clip_f32(a,minBound,maxBound)
109print(a)
110print(b)
111
112a = a / i[0]
113print(a)
114minBound = minBound / i[0]
115maxBound = maxBound / i[0]
116print(minBound,maxBound)
117
118b=dsp.arm_clip_q31(f.toQ31(a),f.toQ31(minBound),f.toQ31(maxBound))
119print(f.Q31toF32(b))
120
121b=dsp.arm_clip_q15(f.toQ15(a),f.toQ15(minBound),f.toQ15(maxBound))
122print(f.Q15toF32(b))
123
124b=dsp.arm_clip_q7(f.toQ7(a),f.toQ7(minBound),f.toQ7(maxBound))
125print(f.Q7toF32(b))
126
127############### MAT VECTOR MULT
128
129printTitle("Matrix x Vector")
130a=np.array([[1.,2,3,4],[5,6,7,8],[9,10,11,12]])
131b=np.array([-2,-1,3,4])
132
133c = np.dot(a,b)
134print(c)
135c = dsp.arm_mat_vec_mult_f32(a,b)
136print(c)
137
138printSubTitle("Fixed point")
139normalizationFactor=2.0*np.sqrt(np.max(np.abs(c)))
140a=a/normalizationFactor
141b=b/normalizationFactor
142print(np.dot(a,b))
143
144c=dsp.arm_mat_vec_mult_q31(f.toQ31(a),f.toQ31(b))
145print(f.Q31toF32(c))
146
147c=dsp.arm_mat_vec_mult_q15(f.toQ15(a),f.toQ15(b))
148print(f.Q15toF32(c))
149
150c=dsp.arm_mat_vec_mult_q7(f.toQ7(a),f.toQ7(b))
151print(f.Q7toF32(c))
152
153############### MATRIX MULTIPLY
154
155printTitle("Matrix x Matrix")
156
157a=np.array([[1.,2,3,4],[5,6,7,8],[9,10,11,12]])
158b=np.array([[1.,2,3],[5.1,6,7],[9.1,10,11],[5,8,4]])
159print(np.dot(a , b))
160c=dsp.arm_mat_mult_f32(a,b)
161print(c[1])
162
163printSubTitle("Fixed point")
164
165printSubTitle(" F32")
166normalizationFactor=2.0*np.sqrt(np.max(np.abs(c[1])))
167a = a / normalizationFactor
168b = b / normalizationFactor
169c=dsp.arm_mat_mult_f32(a,b)
170print(c[1])
171
172print("")
173af = f.toQ31(a)
174bf = f.toQ31(b)
175nbSamples = a.size
176tmp=np.zeros(nbSamples)
177c1 = dsp.arm_mat_mult_q31(af,bf)
178c2 = dsp.arm_mat_mult_opt_q31(af,bf,tmp)
179printSubTitle(" Q31")
180print(f.Q31toF32(c1[1]))
181print(f.Q31toF32(c2[1]))
182
183
184printSubTitle(" Q15")
185print("")
186af = f.toQ15(a)
187bf = f.toQ15(b)
188s=bf.shape
189nb=s[0]*s[1]
190tmp=np.zeros(nb)
191c = dsp.arm_mat_mult_q15(af,bf,tmp)
192print(f.Q15toF32(c[1]))
193
194printSubTitle(" Q7")
195print("")
196af = f.toQ7(a)
197bf = f.toQ7(b)
198s=bf.shape
199nb=s[0]*s[1]
200tmp=np.zeros(nb)
201c = dsp.arm_mat_mult_q7(af,bf,tmp)
202print(f.Q7toF32(c[1]))
203
204################# MAT TRANSPOSE #################
205
206printTitle("Transposition")
207a=np.array([[1.,2,3,4],[5,6,7,8],[9,10,11,12]])
208normalizationFactor=np.max(np.abs(c[1]))
209a = a / normalizationFactor
210
211print(np.transpose(a))
212print("")
213r=dsp.arm_mat_trans_f32(a)
214print(r[1])
215print("")
216
217r=dsp.arm_mat_trans_q31(f.toQ31(a))
218print(f.Q31toF32(r[1]))
219print("")
220
221r=dsp.arm_mat_trans_q15(f.toQ15(a))
222print(f.Q15toF32(r[1]))
223print("")
224
225r=dsp.arm_mat_trans_q7(f.toQ7(a))
226print(f.Q7toF32(r[1]))
227print("")
228
229################## FILL FUNCTIONS #################
230
231v=0.22
232nb=10
233a=np.full((nb,),v)
234print(a)
235
236a=dsp.arm_fill_f32(v,nb)
237print(a)
238
239a=f.Q31toF32(dsp.arm_fill_q31(f.toQ31(v),nb))
240print(a)
241
242a=f.Q15toF32(dsp.arm_fill_q15(f.toQ15(v),nb))
243print(a)
244
245a=f.Q7toF32(dsp.arm_fill_q7(f.toQ7(v),nb))
246print(a)
247
248################# COMPLEX MAT TRANSPOSE #################
249
250printTitle("Complex Transposition")
251a=np.array([[1. + 0.0j ,2 + 1.0j,3 + 0.0j,4 + 2.0j],
252            [5 + 1.0j,6 + 2.0j,7 + 3.0j,8 + 1.0j],
253            [9 - 2.0j,10 + 1.0j,11 - 4.0j,12 + 1.0j]])
254normalizationFactor=np.max(np.abs(c[1]))
255a = a / normalizationFactor
256
257print(np.transpose(a))
258print("")
259r=dsp.arm_mat_cmplx_trans_f32(imToReal2D(a))
260print(realToIm2D(r[1]))
261print("")
262
263r=dsp.arm_mat_cmplx_trans_q31(f.toQ31(imToReal2D(a)))
264print(realToIm2D(f.Q31toF32(r[1])))
265print("")
266
267r=dsp.arm_mat_cmplx_trans_q15(f.toQ15(imToReal2D(a)))
268print(realToIm2D(f.Q15toF32(r[1])))
269print("")
270
271################ Levinson ##################
272
273printTitle("Levinson Durbin")
274na=5
275s = np.random.randn(na+1)
276s = normalize(s)
277phi = autocorr(s)
278phi = normalize(phi)
279
280sigmav,arcoef,pacf,sigma,phi1=statsmodels.tsa.stattools.levinson_durbin(phi,nlags=na,isacov=True)
281
282print(arcoef)
283print(sigmav)
284
285(a,err)=dsp.arm_levinson_durbin_f32(phi,na)
286print(a)
287print(err)
288
289phiQ31 = f.toQ31(phi)
290(aQ31,errQ31)=dsp.arm_levinson_durbin_q31(phiQ31,na)
291print(f.Q31toF32(aQ31))
292print(f.Q31toF32(errQ31))
293
294################## Bitwise operations #################
295
296printTitle("Bitwise operations")
297def genBitvectors(nb,format):
298    if format == 31:
299       maxVal = 0x7fffffff
300    if format == 15:
301       maxVal = 0x7fff
302    if format == 7:
303       maxVal = 0x7f
304
305    minVal = -maxVal-1
306
307    return(np.random.randint(minVal, maxVal, size=nb))
308
309NBSAMPLES=10
310
311
312
313printSubTitle("u32")
314su32A=genBitvectors(NBSAMPLES,31)
315su32B=genBitvectors(NBSAMPLES,31)
316ffff = (np.ones(NBSAMPLES)*(-1)).astype(int)
317
318
319ref=np.bitwise_and(su32A, su32B)
320#print(ref)
321result=dsp.arm_and_u32(su32A, su32B).astype(int)
322print(result-ref)
323
324ref=np.bitwise_or(su32A, su32B)
325#print(ref)
326result=dsp.arm_or_u32(su32A, su32B).astype(int)
327print(result-ref)
328
329ref=np.bitwise_xor(su32A, su32B)
330#print(ref)
331result=dsp.arm_xor_u32(su32A, su32B).astype(int)
332print(result-ref)
333
334ref=np.bitwise_xor(ffff, su32A)
335#print(ref)
336result=dsp.arm_not_u32(su32A).astype(int)
337print(result-ref)
338
339printSubTitle("u16")
340su16A=genBitvectors(NBSAMPLES,15)
341su16B=genBitvectors(NBSAMPLES,15)
342
343ffff = (np.ones(NBSAMPLES)*(-1)).astype(int)
344
345
346ref=np.bitwise_and(su16A, su16B)
347#print(ref)
348result=dsp.arm_and_u16(su16A, su16B).astype(np.short)
349print(result-ref)
350
351ref=np.bitwise_or(su16A, su16B)
352#print(ref)
353result=dsp.arm_or_u16(su16A, su16B).astype(np.short)
354print(result-ref)
355
356ref=np.bitwise_xor(su16A, su16B)
357#print(ref)
358result=dsp.arm_xor_u16(su16A, su16B).astype(np.short)
359print(result-ref)
360
361ref=np.bitwise_xor(ffff, su16A)
362#print(ref)
363result=dsp.arm_not_u16(su16A).astype(np.short)
364print(result-ref)
365
366printSubTitle("u8")
367
368su8A=genBitvectors(NBSAMPLES,7)
369su8B=genBitvectors(NBSAMPLES,7)
370
371ref=np.bitwise_and(su8A, su8B)
372#print(ref)
373result=dsp.arm_and_u8(su8A, su8B).astype(np.byte)
374print(result-ref)
375
376ref=np.bitwise_or(su8A, su8B)
377#print(ref)
378result=dsp.arm_or_u8(su8A, su8B).astype(np.byte)
379print(result-ref)
380
381ref=np.bitwise_xor(su8A, su8B)
382#print(ref)
383result=dsp.arm_xor_u8(su8A, su8B).astype(np.byte)
384print(result-ref)
385
386ref=np.bitwise_xor(ffff, su8A)
387#print(ref)
388result=dsp.arm_not_u8(su8A).astype(np.byte)
389print(result-ref)
390
391#################### Quaternion tests ##################
392NBSAMPLES=3
393
394def flattenQuat(l):
395    return(np.array([list(x) for x in l]).reshape(4*len(l)))
396
397def flattenRot(l):
398    return(np.array([list(x) for x in l]).reshape(9*len(l)))
399
400# q and -q are representing the same rotation.
401# So there is an ambiguity for the tests.
402# We force the real part of be positive.
403def mkQuaternion(mat):
404    q=Quaternion(matrix=mat)
405    if q.scalar < 0:
406        return(-q)
407    else:
408        return(q)
409
410a=[2.0*Quaternion.random() for x in range(NBSAMPLES)]
411src=flattenQuat(a)
412
413
414res=flattenQuat([x.normalised for x in a])
415print(res)
416output=dsp.arm_quaternion_normalize_f32(src)
417print(output)
418print("")
419
420res=flattenQuat([x.conjugate for x in a])
421print(res)
422output=dsp.arm_quaternion_conjugate_f32(src)
423print(output)
424print("")
425
426res=flattenQuat([x.inverse for x in a])
427print(res)
428output=dsp.arm_quaternion_inverse_f32(src)
429print(output)
430print("")
431
432res=[x.norm for x in a]
433print(res)
434output=dsp.arm_quaternion_norm_f32(src)
435print(output)
436print("")
437
438a=[x.normalised for x in a]
439ra=[x.rotation_matrix for x in a]
440rb=[mkQuaternion(x) for x in ra]
441
442srca=flattenQuat(a)
443resa=dsp.arm_quaternion2rotation_f32(srca)
444resb=dsp.arm_rotation2quaternion_f32(resa)
445
446
447print(ra)
448print(resa)
449print("")
450print(rb)
451print(resb)#
452
453a=[2.0*Quaternion.random() for x in range(NBSAMPLES)]
454b=[2.0*Quaternion.random() for x in range(NBSAMPLES)]
455
456c = np.array(a) * np.array(b)
457print(c)
458
459srca=flattenQuat(a)
460srcb=flattenQuat(b)
461resc=dsp.arm_quaternion_product_f32(srca,srcb)
462
463print(resc)
464
465print(a[0]*b[0])
466res=dsp.arm_quaternion_product_single_f32(srca[0:4],srcb[0:4])
467print(res)
468
469