1# New functions for version 1.4 of the Python wrapper
2import cmsisdsp as dsp
3import cmsisdsp.fixedpoint as f
4import numpy as np
5import math
6import colorama
7from colorama import init,Fore, Back, Style
8
9init()
10
11def printTitle(s):
12    print("\n" + Fore.GREEN + Style.BRIGHT +  s + Style.RESET_ALL)
13
14def printSubTitle(s):
15    print("\n" + Style.BRIGHT + s + Style.RESET_ALL)
16
17printTitle("ArcTan2")
18
19angles=[0,
20math.pi/4,
21math.pi/2,
223*math.pi/4,
23math.pi,
245*math.pi/4,
253*math.pi/2,
267*math.pi/4
27]
28
29x = np.cos(angles)
30y = np.sin(angles)
31
32vals=list(zip(y,x))
33
34printSubTitle("Atan2 Reference")
35ref=np.array([np.arctan2(yv,xv) for (yv,xv) in vals])/math.pi*180
36print(ref)
37
38printSubTitle("Atan2 F32")
39resf32=np.array([dsp.arm_atan2_f32(yv,xv)[1]  for (yv,xv) in vals])/math.pi*180
40print(resf32)
41print(np.isclose(ref,resf32,1e-6,1e-6))
42
43printSubTitle("Atan2 Q31")
44xQ31=f.toQ31(x)
45yQ31=f.toQ31(y)
46valsQ31=list(zip(yQ31,xQ31))
47
48resq31=4*f.Q31toF32(np.array([dsp.arm_atan2_q31(yv,xv)[1]  for (yv,xv) in valsQ31]))/math.pi*180
49print(resq31)
50print(np.isclose(ref,resq31,1e-8,1e-8))
51
52printSubTitle("Atan2 Q15")
53xQ15=f.toQ15(x)
54yQ15=f.toQ15(y)
55valsQ15=list(zip(yQ15,xQ15))
56
57resq15=4*f.Q15toF32(np.array([dsp.arm_atan2_q15(yv,xv)[1]  for (yv,xv) in valsQ15]))/math.pi*180
58print(resq15)
59print(np.isclose(ref,resq15,1e-3,1e-3))
60
61printTitle("MSE")
62
63NBSAMPLES = 50
64
65def mse(a,b):
66    err = a - b
67    return(np.dot(err,err) / len(a))
68
69a=np.random.randn(NBSAMPLES)
70a = a / np.max(np.abs(a))
71
72b=np.random.randn(NBSAMPLES)
73b = b / np.max(np.abs(b))
74
75printSubTitle("MSE Reference")
76ref = mse(a,b)
77print(ref)
78
79printSubTitle("MSE f64")
80resf64= dsp.arm_mse_f64(a,b)
81print(resf64)
82print(np.isclose(ref,resf64,1e-14,1e-14))
83
84printSubTitle("MSE f32")
85resf32 = dsp.arm_mse_f32(a,b)
86print(resf32)
87print(np.isclose(ref,resf32,1e-7,1e-7))
88
89printSubTitle("MSE Q31")
90aQ31 = f.toQ31(a)
91bQ31 = f.toQ31(b)
92
93resQ31 = f.Q31toF32(dsp.arm_mse_q31(aQ31,bQ31))
94print(resQ31)
95print(np.isclose(ref,resQ31,1e-7,1e-7))
96
97aQ15 = f.toQ15(a)
98bQ15 = f.toQ15(b)
99
100printSubTitle("MSE Q15")
101resQ15 = dsp.arm_mse_q15(aQ15,bQ15)
102print("%04X" % resQ15)
103resQ15 = f.Q15toF32(resQ15)
104print(resQ15)
105print(np.isclose(ref,resQ15,1e-4,1e-4))
106
107aQ7 = f.toQ7(a)
108bQ7 = f.toQ7(b)
109
110printSubTitle("MSE Q7")
111resQ7 = dsp.arm_mse_q7(aQ7,bQ7)
112print("%04X" % resQ7)
113resQ7 = f.Q7toF32(resQ7)
114print(resQ7)
115print(np.isclose(ref,resQ7,1e-2,1e-2))
116
117