1 /*
2 * Copyright (c) 2022 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #ifndef HPM_BLDC_MATH_H
8 #define HPM_BLDC_MATH_H
9
10 /**
11 * @addtogroup mcl_math_interface HPMicro MCL MATH APIs
12 * @ingroup middleware_mcl_interfaces
13 * @{
14 *
15 */
16
17 /**
18 * @brief Motor library using mathematical format
19 *
20 */
21 #define HPM_MOTOR_MATH_MOD_Q_SW (1) /**< Qmath software */
22 #define HPM_MOTOR_MATH_MOD_Q_HW (2) /**< Qmath hardware */
23 #define HPM_MOTOR_MATH_MOD_DSP_FP (3) /**< dsp hardware floating point unit */
24 #define HPM_MOTOR_MATH_MOD_Q_ALL (4) /**< Qmath soft ware and hardware */
25 #define HPM_MOTOR_MATH_MOD_FP (5) /**< hardware floating point unit */
26
27 /**
28 * @brief Q-format
29 *
30 */
31 #define HPM_QMATH_MOD_Q15 (15)
32 #define HPM_QMATH_MOD_Q31 (31)
33
34 /**
35 * @brief User-defined profiles
36 *
37 */
38 #ifdef CONFIG_MATH_HAS_EXTRA_CONFIG
39 #include CONFIG_MATH_HAS_EXTRA_CONFIG
40 #endif
41
42 #ifdef __cplusplus
43 extern "C"
44 {
45 #endif
46
47 #ifndef HPM_MOTOR_MATH_MOD
48 #define HPM_MOTOR_MATH_MOD HPM_MOTOR_MATH_MOD_FP
49 #endif
50
51 #ifndef HPM_QMATH_N
52 #define HPM_QMATH_N HPM_QMATH_MOD_Q31
53 #endif
54
55 #ifndef HPM_SOFT_Q_N
56 #define HPM_SOFT_Q_N (15)
57 #endif
58
59 #include <stdint.h>
60 #if HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_SW
61 typedef int32_t HPM_MOTOR_MATH_TYPE;
62 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_HW
63 #ifndef HPM_QMATH_N
64 #define HPM_QMATH_N HPM_QMATH_MOD_Q31
65 #endif
66 #if (HPM_QMATH_N == HPM_QMATH_MOD_Q15)
67 typedef int16_t HPM_MOTOR_MATH_TYPE;/*Q15格式*/
68 #elif (HPM_QMATH_N == HPM_QMATH_MOD_Q31)
69 typedef int32_t HPM_MOTOR_MATH_TYPE;/*Q31格式*/
70 #endif
71 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_DSP_FP
72 typedef float HPM_MOTOR_MATH_TYPE;
73 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_FP
74 typedef float HPM_MOTOR_MATH_TYPE;
75 #endif
76
77 /**
78 * @brief Floating point to Q format conversion
79 *
80 * @param[in] from_f32 Floating point
81 * @return @ref HPM_MOTOR_MATH_TYPE
82 */
83 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_f32_q(float from_f32);
84
85 /**
86 * @brief Q format to floating point conversion
87 *
88 * @param[in] from_mdf Q format
89 * @return float
90 */
91 float hpm_dsp_hw_q_f32(HPM_MOTOR_MATH_TYPE from_mdf);
92
93 /**
94 * @brief Q-format multiplication
95 *
96 * @param[in] x data
97 * @param[in] y data
98 * @return x*y
99 */
100 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_mul_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
101
102 /**
103 * @brief Q-format division
104 *
105 * @param[in] x data
106 * @param[in] y data
107 * @return x/y
108 */
109 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_div_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
110
111 /**
112 * @brief Q-format atan
113 *
114 * @param[in] x data
115 * @return atan(x)
116 */
117 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan_q(HPM_MOTOR_MATH_TYPE x);
118
119 /**
120 * @brief Q-format atan2
121 *
122 * @param[in] x data
123 * @param[in] y data
124 * @return atan(x, y)
125 */
126 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan2_q(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
127
128 /**
129 * @brief Q-format absolute
130 *
131 * @param[in] x data
132 * @return |data|
133 */
134 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_abs_q(HPM_MOTOR_MATH_TYPE x);
135
136 /**
137 * @brief Floating-point multiplication
138 *
139 * @param[in] x data
140 * @param[in] y data
141 * @return x*y
142 */
143 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_mul_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
144
145 /**
146 * @brief Floating-point division
147 *
148 * @param[in] x data
149 * @param[in] y data
150 * @return x/y
151 */
152 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_div_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
153
154 /**
155 * @brief Floating-point atan
156 *
157 * @param[in] x data
158 * @return atan(x)
159 */
160 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan_fp(HPM_MOTOR_MATH_TYPE x);
161
162 /**
163 * @brief Floating-point atan2
164 *
165 * @param[in] x data
166 * @param[in] y data
167 * @return atan2(x,y)
168 */
169 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_atan2_fp(HPM_MOTOR_MATH_TYPE x, HPM_MOTOR_MATH_TYPE y);
170
171 /**
172 * @brief Floating-point absolute
173 *
174 * @param[in] x data
175 * @return |x|
176 */
177 HPM_MOTOR_MATH_TYPE hpm_dsp_hw_abs_fp(HPM_MOTOR_MATH_TYPE x);
178
soft_conv_float_to_q31(float x)179 static inline HPM_MOTOR_MATH_TYPE soft_conv_float_to_q31(float x)
180 {
181 HPM_MOTOR_MATH_TYPE q31;
182 int64_t q64;
183 double x1 = x;
184 x1 *= (1 << HPM_SOFT_Q_N);
185 x1 += (x < 0.0f ? -0.5f : 0.5f);
186 q64 = (HPM_MOTOR_MATH_TYPE)x1;
187 if (q64 >= INT32_MAX) {
188 q64 = INT32_MAX;
189 } else if (q64 <= INT32_MIN) {
190 q64 = INT32_MIN;
191 } else {
192 q31 = (HPM_MOTOR_MATH_TYPE)q64;
193 }
194 return q31;
195 }
soft_conv_q31_to_float(HPM_MOTOR_MATH_TYPE x)196 static inline float soft_conv_q31_to_float(HPM_MOTOR_MATH_TYPE x)
197 {
198 float val;
199 val = (double)x/(1<<HPM_SOFT_Q_N);
200 return val;
201 }
202
203 #if HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_SW
204 /*软件*/
205 #if (HPM_QMATH_N != HPM_QMATH_MOD_Q31)
206 #error "SOFT Q_MATH only support q31"
207 #endif
208 #include <math.h>
209 #define HPM_MOTOR_MATH_FL_MDF(from_f32) soft_conv_float_to_q31(from_f32)
210 #define HPM_MOTOR_MATH_MDF_FL(from_mdf) soft_conv_q31_to_float(from_mdf)
211 #define HPM_MOTOR_MATH_MUL(x, y) ((((int64_t)x)*y)>>HPM_SOFT_Q_N)
212 #define HPM_MOTOR_MATH_DIV(x, y) ((((int64_t)x)<<HPM_SOFT_Q_N)/y)
213 #define HPM_MOTOR_MATH_ATAN2(x, y) atan2f(x, y)
214 #define HPM_MOTOR_MATH_FABS(x) abs(x)
215 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_Q_HW
216 /*Qmath*/
217 #define HPM_MOTOR_MATH_FL_MDF(from_f32) hpm_dsp_hw_f32_q(from_f32)
218 #define HPM_MOTOR_MATH_MDF_FL(from_mdf) hpm_dsp_hw_q_f32(from_mdf)
219 #define HPM_MOTOR_MATH_MUL(x, y) hpm_dsp_hw_mul_q(x, y)
220 #define HPM_MOTOR_MATH_DIV(x, y) hpm_dsp_hw_div_q(x, y)
221 #define HPM_MOTOR_MATH_ATAN2(x, y) hpm_dsp_hw_atan2_q(x, y)
222 #define HPM_MOTOR_MATH_ATAN(x) hpm_dsp_hw_atan_q(x)
223 #define HPM_MOTOR_MATH_FABS(x) hpm_dsp_hw_abs_q(x)
224
225 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_DSP_FP
226 #define HPM_MOTOR_MATH_FL_MDF(from_f32) (from_f32)
227 #define HPM_MOTOR_MATH_MDF_FL(from_mdf) (from_mdf)
228 #define HPM_MOTOR_MATH_MUL(x, y) hpm_dsp_hw_mul_fp(x, y)
229 #define HPM_MOTOR_MATH_DIV(x, y) hpm_dsp_hw_div_fp(x, y)
230 #define HPM_MOTOR_MATH_ATAN2(x, y) hpm_dsp_hw_atan2_fp(x, y)
231 #define HPM_MOTOR_MATH_ATAN(x) hpm_dsp_hw_atan_fp(x)
232 #define HPM_MOTOR_MATH_FABS(x) hpm_dsp_hw_abs_fp(x)
233 #elif HPM_MOTOR_MATH_MOD == HPM_MOTOR_MATH_MOD_FP
234 #include <math.h>
235 #define HPM_MOTOR_MATH_FL_MDF(from_f32) (from_f32)
236 #define HPM_MOTOR_MATH_MDF_FL(from_mdf) (from_mdf)
237 #define HPM_MOTOR_MATH_MUL(x, y) (x*y)
238 #define HPM_MOTOR_MATH_DIV(x, y) (x/y)
239 #define HPM_MOTOR_MATH_ATAN2(x, y) atan2(x, y)
240 #define HPM_MOTOR_MATH_ATAN(x) atan(x)
241 #define HPM_MOTOR_MATH_FABS(x) abs(x)
242 #endif
243
244 #ifdef __cplusplus
245 }
246 #endif
247
248 /**
249 * @}
250 *
251 */
252
253 #endif
254