1 /*
2 * Copyright (c) 2022 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #include "hpm_motor_math.h"
8 #if (HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_HW) || (HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_DSP_FP)
9 #include "hpm_math.h"
10 #endif
11
12 #if HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_HW
hpm_dsp_hw_f32_q(float from_f32)13 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_f32_q(float from_f32)
14 {
15 HPM_MOTOR_MATH_TYPE m_val;
16 float from_f321 = from_f32;
17 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
18 hpm_dsp_convert_f32_q15(&from_f321, &m_val, 1);
19 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
20 hpm_dsp_convert_f32_q31(&from_f321, &m_val, 1);
21 #endif
22 return m_val;
23 }
24
hpm_dsp_hw_q_f32(HPM_MOTOR_MATH_TYPE from_mdf)25 float hpm_dsp_hw_q_f32(HPM_MOTOR_MATH_TYPE from_mdf)
26 {
27 float m_val;
28 HPM_MOTOR_MATH_TYPE from_q1 = from_mdf;
29 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
30 hpm_dsp_convert_q15_f32(&from_q1, &m_val, 1);
31 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
32 hpm_dsp_convert_q31_f32(&from_q1, &m_val, 1);
33 #endif
34 return m_val;
35 }
36
hpm_dsp_hw_mul_q(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)37 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_mul_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
38 {
39 HPM_MOTOR_MATH_TYPE m_val;
40 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
41 hpm_dsp_mul_q15(&x, &y, &m_val, 1);
42 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
43 hpm_dsp_mul_q31(&x, &y, &m_val, 1);
44 #endif
45 return m_val;
46 }
47
hpm_dsp_hw_div_q(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)48 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_div_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
49 {
50 HPM_MOTOR_MATH_TYPE m_val;
51 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
52 m_val = ((((int)x)<<15)/y);
53 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
54 hpm_dsp_mul_q31(&x, &y, &m_val, 1);
55 #endif
56 return m_val;
57 }
58
hpm_dsp_hw_atan_q(HPM_MOTOR_MATH_TYPE x)59 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan_q(HPM_MOTOR_MATH_TYPE x)
60 {
61 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
62 return hpm_dsp_atan_q15(x);
63 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
64 return hpm_dsp_atan_q31(x);
65 #endif
66 }
67
hpm_dsp_hw_atan2_q(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)68 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan2_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
69 {
70 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
71 return hpm_dsp_atan2_q15(x, y);
72 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
73 return hpm_dsp_atan2_q31(x, y);
74 #endif
75 }
76
hpm_dsp_hw_abs_q(HPM_MOTOR_MATH_TYPE x)77 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_abs_q(HPM_MOTOR_MATH_TYPE x)
78 {
79 HPM_MOTOR_MATH_TYPE m_val;
80 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
81 hpm_dsp_abs_q15(&x, &m_val, 1);
82 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
83 hpm_dsp_abs_q31(&x, &m_val, 1);
84 #endif
85 return m_val;
86 }
87 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_DSP_FP
88
hpm_dsp_hw_mul_fp(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)89 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_mul_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
90 {
91 HPM_MOTOR_MATH_TYPE m_val;
92 hpm_dsp_mul_f32(&x, &y, &m_val, 1);
93 return m_val;
94 }
95
hpm_dsp_hw_div_fp(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)96 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_div_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
97 {
98 HPM_MOTOR_MATH_TYPE m_val;
99 hpm_dsp_div_f32(&x, &y, &m_val, 1);
100 return m_val;
101 }
102
hpm_dsp_hw_atan_fp(HPM_MOTOR_MATH_TYPE x)103 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan_fp(HPM_MOTOR_MATH_TYPE x)
104 {
105 return hpm_dsp_atan_f32(x);
106 }
107
hpm_dsp_hw_atan2_fp(HPM_MOTOR_MATH_TYPE x,HPM_MOTOR_MATH_TYPE y)108 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan2_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y)
109 {
110 return hpm_dsp_atan2_f32(x, y);
111 }
112
hpm_dsp_hw_abs_fp(HPM_MOTOR_MATH_TYPE x)113 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_abs_fp(HPM_MOTOR_MATH_TYPE x)
114 {
115 HPM_MOTOR_MATH_TYPE m_val;
116 hpm_dsp_abs_f32(&x, &m_val, 1);
117 return m_val;
118 }
119
120 #endif
121
122