1 /*
2 * Copyright (c) 2021 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_common.h"
9 #include "hpm_bldc_define.h"
10 #include "hpm_foc.h"
11 #include "hpm_smc.h"
12
hpm_mcl_nullcallback_func(void)13 void hpm_mcl_nullcallback_func(void)
14 {
15 while (1) {
16 ;
17 }
18 }
19 /*
20 *
21 *
22 * #include <stdio.h>
23 * #include <math.h>
24 * #define NUM 501
25 * #define PRECISION 0.18
26 * #define PI 3.14159265
27 * int main()
28 * {
29 * short i;
30 * printf("staitc const float bldc_foc_sintable[%d] =\r",NUM);
31 * printf("{\r");
32 * for(i=0;i<NUM;i++) {
33 * printf(" %f",sin(i*PRECISION*PI/180));
34 * if(i != NUM-1) {
35 * printf(",");
36 * }
37 *
38 * if((i%10) == 0) {
39 * printf("\r");
40 * }
41 * }
42 * printf("};\r");
43 *
44 * return 0;
45 * }
46 *
47 */
48 #define PRECISION (0.18)
49 #define SIN_TABLE_INDEX_MAX (500)
50 const float bldc_foc_sintable[SIN_TABLE_INDEX_MAX + 1] = {
51
52 0.000000,
53 0.003142, 0.006283, 0.009425, 0.012566, 0.015707, 0.018848, 0.021989, 0.025130, 0.028271, 0.031411,
54 0.034551, 0.037690, 0.040829, 0.043968, 0.047106, 0.050244, 0.053382, 0.056519, 0.059655, 0.062791,
55 0.065926, 0.069060, 0.072194, 0.075327, 0.078459, 0.081591, 0.084721, 0.087851, 0.090980, 0.094108,
56 0.097235, 0.100362, 0.103487, 0.106611, 0.109734, 0.112856, 0.115977, 0.119097, 0.122216, 0.125333,
57 0.128449, 0.131564, 0.134678, 0.137790, 0.140901, 0.144011, 0.147119, 0.150226, 0.153331, 0.156434,
58 0.159537, 0.162637, 0.165736, 0.168833, 0.171929, 0.175023, 0.178115, 0.181206, 0.184294, 0.187381,
59 0.190466, 0.193549, 0.196631, 0.199710, 0.202787, 0.205863, 0.208936, 0.212007, 0.215076, 0.218143,
60 0.221208, 0.224271, 0.227331, 0.230389, 0.233445, 0.236499, 0.239550, 0.242599, 0.245646, 0.248690,
61 0.251732, 0.254771, 0.257807, 0.260842, 0.263873, 0.266902, 0.269928, 0.272952, 0.275973, 0.278991,
62 0.282007, 0.285019, 0.288029, 0.291036, 0.294040, 0.297042, 0.300040, 0.303035, 0.306028, 0.309017,
63 0.312003, 0.314987, 0.317967, 0.320944, 0.323917, 0.326888, 0.329855, 0.332820, 0.335780, 0.338738,
64 0.341692, 0.344643, 0.347590, 0.350534, 0.353475, 0.356412, 0.359345, 0.362275, 0.365202, 0.368125,
65 0.371044, 0.373959, 0.376871, 0.379779, 0.382683, 0.385584, 0.388481, 0.391374, 0.394263, 0.397148,
66 0.400029, 0.402906, 0.405780, 0.408649, 0.411514, 0.414376, 0.417233, 0.420086, 0.422935, 0.425779,
67 0.428620, 0.431456, 0.434288, 0.437116, 0.439939, 0.442758, 0.445573, 0.448383, 0.451189, 0.453990,
68 0.456787, 0.459580, 0.462368, 0.465151, 0.467930, 0.470704, 0.473473, 0.476238, 0.478998, 0.481754,
69 0.484504, 0.487250, 0.489991, 0.492727, 0.495459, 0.498185, 0.500907, 0.503623, 0.506335, 0.509041,
70 0.511743, 0.514440, 0.517131, 0.519817, 0.522499, 0.525175, 0.527846, 0.530511, 0.533172, 0.535827,
71 0.538477, 0.541121, 0.543760, 0.546394, 0.549023, 0.551646, 0.554263, 0.556876, 0.559482, 0.562083,
72 0.564679, 0.567269, 0.569853, 0.572432, 0.575005, 0.577573, 0.580134, 0.582690, 0.585241, 0.587785,
73 0.590324, 0.592857, 0.595384, 0.597905, 0.600420, 0.602930, 0.605433, 0.607930, 0.610422, 0.612907,
74 0.615386, 0.617860, 0.620327, 0.622788, 0.625243, 0.627691, 0.630134, 0.632570, 0.635000, 0.637424,
75 0.639841, 0.642253, 0.644657, 0.647056, 0.649448, 0.651834, 0.654213, 0.656586, 0.658952, 0.661312,
76 0.663665, 0.666012, 0.668352, 0.670686, 0.673013, 0.675333, 0.677646, 0.679953, 0.682254, 0.684547,
77 0.686834, 0.689114, 0.691387, 0.693653, 0.695913, 0.698165, 0.700411, 0.702650, 0.704882, 0.707107,
78 0.709325, 0.711536, 0.713740, 0.715936, 0.718126, 0.720309, 0.722485, 0.724653, 0.726814, 0.728969,
79 0.731116, 0.733255, 0.735388, 0.737513, 0.739631, 0.741742, 0.743845, 0.745941, 0.748030, 0.750111,
80 0.752185, 0.754251, 0.756310, 0.758362, 0.760406, 0.762443, 0.764472, 0.766493, 0.768507, 0.770513,
81 0.772512, 0.774503, 0.776487, 0.778462, 0.780430, 0.782391, 0.784343, 0.786288, 0.788226, 0.790155,
82 0.792077, 0.793990, 0.795896, 0.797794, 0.799685, 0.801567, 0.803441, 0.805308, 0.807166, 0.809017,
83 0.810860, 0.812694, 0.814521, 0.816339, 0.818150, 0.819952, 0.821746, 0.823533, 0.825311, 0.827081,
84 0.828842, 0.830596, 0.832341, 0.834078, 0.835807, 0.837528, 0.839240, 0.840945, 0.842640, 0.844328,
85 0.846007, 0.847678, 0.849340, 0.850994, 0.852640, 0.854277, 0.855906, 0.857527, 0.859139, 0.860742,
86 0.862337, 0.863923, 0.865501, 0.867071, 0.868632, 0.870184, 0.871727, 0.873262, 0.874789, 0.876307,
87 0.877816, 0.879316, 0.880808, 0.882291, 0.883766, 0.885231, 0.886688, 0.888136, 0.889576, 0.891007,
88 0.892428, 0.893841, 0.895246, 0.896641, 0.898028, 0.899405, 0.900774, 0.902134, 0.903485, 0.904827,
89 0.906160, 0.907484, 0.908800, 0.910106, 0.911403, 0.912692, 0.913971, 0.915241, 0.916502, 0.917755,
90 0.918998, 0.920232, 0.921457, 0.922673, 0.923880, 0.925077, 0.926266, 0.927445, 0.928615, 0.929776,
91 0.930928, 0.932071, 0.933205, 0.934329, 0.935444, 0.936550, 0.937646, 0.938734, 0.939812, 0.940881,
92 0.941940, 0.942991, 0.944031, 0.945063, 0.946085, 0.947098, 0.948102, 0.949096, 0.950081, 0.951057,
93 0.952023, 0.952979, 0.953927, 0.954865, 0.955793, 0.956712, 0.957622, 0.958522, 0.959412, 0.960294,
94 0.961165, 0.962028, 0.962880, 0.963724, 0.964557, 0.965382, 0.966196, 0.967001, 0.967797, 0.968583,
95 0.969360, 0.970127, 0.970884, 0.971632, 0.972370, 0.973099, 0.973817, 0.974527, 0.975227, 0.975917,
96 0.976597, 0.977268, 0.977929, 0.978581, 0.979223, 0.979855, 0.980478, 0.981091, 0.981694, 0.982287,
97 0.982871, 0.983445, 0.984010, 0.984564, 0.985109, 0.985645, 0.986170, 0.986686, 0.987192, 0.987688,
98 0.988175, 0.988652, 0.989119, 0.989576, 0.990024, 0.990461, 0.990889, 0.991308, 0.991716, 0.992115,
99 0.992504, 0.992883, 0.993252, 0.993611, 0.993961, 0.994301, 0.994631, 0.994951, 0.995261, 0.995562,
100 0.995853, 0.996134, 0.996405, 0.996666, 0.996917, 0.997159, 0.997391, 0.997613, 0.997825, 0.998027,
101 0.998219, 0.998402, 0.998574, 0.998737, 0.998890, 0.999033, 0.999166, 0.999289, 0.999403, 0.999507,
102 0.999600, 0.999684, 0.999758, 0.999822, 0.999877, 0.999921, 0.999956, 0.999980, 0.999995, 1.000000
103 };
104
hpm_mcl_bldc_foc_al_speed(BLDC_CONTRL_SPD_PARA * par)105 void hpm_mcl_bldc_foc_al_speed(BLDC_CONTRL_SPD_PARA *par)
106 {
107 HPM_MOTOR_MATH_TYPE delta;
108 delta = par->speedtheta - par->speedlasttheta;
109 if (delta > HPM_MOTOR_MATH_FL_MDF(180)) {/*-speed*/
110 delta = -par->speedlasttheta - (HPM_MOTOR_MATH_FL_MDF(360) - par->speedtheta);
111 } else if (delta < HPM_MOTOR_MATH_FL_MDF(-180)) {/*+speed*/
112 delta = HPM_MOTOR_MATH_FL_MDF(360) + par->speedtheta - par->speedlasttheta;
113 }
114 par->speedthetalastn += delta;
115 par->speedlasttheta = par->speedtheta;
116 par->num++;
117 if (par->i_speedacq == par->num) {
118 par->num = 0;
119 par->o_speedout = HPM_MOTOR_MATH_DIV(par->speedthetalastn,
120 HPM_MOTOR_MATH_MUL(HPM_MOTOR_MATH_MUL(par->i_speedlooptime_s, HPM_MOTOR_MATH_FL_MDF(par->i_motorpar->i_poles_n)), HPM_MOTOR_MATH_FL_MDF(360)));
121 par->o_speedout_filter = par->o_speedout_filter + HPM_MOTOR_MATH_MUL(par->i_speedfilter,
122 (par->o_speedout - par->o_speedout_filter));
123 par->speedthetalastn = 0;
124 }
125 }
126
bldc_foc_sin_cos(HPM_MOTOR_MATH_TYPE angle,HPM_MOTOR_MATH_TYPE angle_precision,HPM_MOTOR_MATH_TYPE * sin_angle,HPM_MOTOR_MATH_TYPE * cos_angle)127 static void bldc_foc_sin_cos(HPM_MOTOR_MATH_TYPE angle, HPM_MOTOR_MATH_TYPE angle_precision,
128 HPM_MOTOR_MATH_TYPE *sin_angle, HPM_MOTOR_MATH_TYPE *cos_angle)
129 {
130 int16_t transfer = 0;
131 uint16_t angle_int = HPM_MOTOR_MATH_MDF_FL(angle);
132
133 if (angle_int < HPM_MOTOR_MATH_FL_MDF(90)) {
134 transfer = HPM_MOTOR_MATH_MDF_FL(HPM_MOTOR_MATH_DIV(angle, angle_precision));
135 *sin_angle = HPM_MOTOR_MATH_FL_MDF(bldc_foc_sintable[transfer]);
136 *cos_angle = HPM_MOTOR_MATH_FL_MDF(bldc_foc_sintable[SIN_TABLE_INDEX_MAX - transfer]);
137 } else if (angle_int < HPM_MOTOR_MATH_FL_MDF(180)) {
138 transfer = HPM_MOTOR_MATH_MDF_FL(HPM_MOTOR_MATH_DIV((HPM_MOTOR_MATH_FL_MDF(180) - angle), angle_precision));
139 *sin_angle = HPM_MOTOR_MATH_FL_MDF(bldc_foc_sintable[transfer]);
140 *cos_angle = HPM_MOTOR_MATH_FL_MDF(-bldc_foc_sintable[SIN_TABLE_INDEX_MAX - transfer]);
141 } else if (angle_int < HPM_MOTOR_MATH_FL_MDF(270)) {
142 transfer = HPM_MOTOR_MATH_MDF_FL(HPM_MOTOR_MATH_DIV((angle - HPM_MOTOR_MATH_FL_MDF(180)), angle_precision));
143 *sin_angle = HPM_MOTOR_MATH_FL_MDF(-bldc_foc_sintable[transfer]);
144 *cos_angle = HPM_MOTOR_MATH_FL_MDF(-bldc_foc_sintable[SIN_TABLE_INDEX_MAX - transfer]);
145 } else if (angle_int <= HPM_MOTOR_MATH_FL_MDF(360)) {
146 transfer = HPM_MOTOR_MATH_MDF_FL(HPM_MOTOR_MATH_DIV((HPM_MOTOR_MATH_FL_MDF(360)-angle), angle_precision));
147 *sin_angle = HPM_MOTOR_MATH_FL_MDF(-bldc_foc_sintable[transfer]);
148 *cos_angle = HPM_MOTOR_MATH_FL_MDF(bldc_foc_sintable[SIN_TABLE_INDEX_MAX - transfer]);
149 }
150 }
151
hpm_mcl_bldc_foc_inv_park(HPM_MOTOR_MATH_TYPE ud,HPM_MOTOR_MATH_TYPE uq,HPM_MOTOR_MATH_TYPE * ualpha,HPM_MOTOR_MATH_TYPE * ubeta,HPM_MOTOR_MATH_TYPE sin_angle,HPM_MOTOR_MATH_TYPE cos_angle)152 void hpm_mcl_bldc_foc_inv_park(HPM_MOTOR_MATH_TYPE ud, HPM_MOTOR_MATH_TYPE uq,
153 HPM_MOTOR_MATH_TYPE *ualpha, HPM_MOTOR_MATH_TYPE *ubeta,
154 HPM_MOTOR_MATH_TYPE sin_angle, HPM_MOTOR_MATH_TYPE cos_angle)
155 {
156 *ualpha = HPM_MOTOR_MATH_MUL(cos_angle, ud) + HPM_MOTOR_MATH_MUL(-sin_angle, uq);
157 *ubeta = HPM_MOTOR_MATH_MUL(sin_angle, ud) + HPM_MOTOR_MATH_MUL(cos_angle, uq);
158 }
159
hpm_mcl_bldc_foc_svpwm(BLDC_CONTROL_PWM_PARA * par)160 void hpm_mcl_bldc_foc_svpwm(BLDC_CONTROL_PWM_PARA *par)
161 {
162 int32_t ualpha_60, ubeta_30;
163 uint32_t pwm_reload;
164 int32_t uref1, uref2, uref3;
165 int32_t tx, ty, t0;
166 int32_t tuon, tvon, twon = 0;
167 int8_t sector = 0;
168
169 uref1 = HPM_MOTOR_MATH_MDF_FL(par->target_beta);
170 ualpha_60 = HPM_MOTOR_MATH_MDF_FL(par->target_alpha);
171 ubeta_30 = HPM_MOTOR_MATH_MDF_FL(par->target_beta);
172 ualpha_60 = (ualpha_60 * 14189) >> 14;
173 ubeta_30 = ubeta_30 >> 1;
174 uref2 = HPM_MOTOR_MATH_MDF_FL(ualpha_60 - ubeta_30);
175 uref3 = HPM_MOTOR_MATH_MDF_FL(-ualpha_60 - ubeta_30);
176 pwm_reload = par->pwmout.i_pwm_reload;
177
178 if (uref1 >= 0)
179 sector = 1;
180 if (uref2 >= 0)
181 sector = sector + 2;
182 if (uref3 >= 0)
183 sector = sector + 4;
184
185 switch (sector) {
186 case 1: /* sector2 000 010 110 111 110 010 000 */
187 tx = -uref2;
188 ty = -uref3;
189 t0 = pwm_reload - tx - ty;
190 /* tx + ty > T */
191 if (t0 < 0) {
192 tx = ((float)tx / (tx + ty)) * pwm_reload;
193 ty = pwm_reload - tx;
194 t0 = 0;
195 }
196 twon = t0 >> 1;
197 tuon = ty + twon;
198 tvon = tx + tuon;
199 break;
200 case 2: /* sector6 000 100 101 111 101 100 000 */
201 tx = -uref3;
202 ty = -uref1;
203 t0 = pwm_reload - tx - ty;
204 if (t0 < 0) {
205 tx = ((float)tx / (tx + ty)) * pwm_reload;
206 ty = pwm_reload - tx;
207 t0 = 0;
208 }
209 tvon = t0 >> 1;
210 twon = ty + tvon;
211 tuon = tx + twon;
212 break;
213 case 3: /* sector1 000 100 110 111 110 100 000 */
214 tx = uref2;
215 ty = uref1;
216 t0 = pwm_reload - tx - ty;
217 if (t0 < 0) {
218 tx = ((float)tx / (tx + ty)) * pwm_reload;
219 ty = pwm_reload - tx;
220 t0 = 0;
221 }
222 twon = t0 >> 1;
223 tvon = ty + twon;
224 tuon = tx + tvon;
225 break;
226 case 4: /* sector4 000 001 011 111 011 001 000 */
227 tx = -uref1;
228 ty = -uref2;
229 t0 = pwm_reload - tx - ty;
230 if (t0 < 0) {
231 tx = ((float)tx / (tx + ty)) * pwm_reload;
232 ty = pwm_reload - tx;
233 t0 = 0;
234 }
235 tuon = t0 >> 1;
236 tvon = ty + tuon;
237 twon = tx + tvon;
238 break;
239 case 5: /* sector5 000 010 011 111 011 010 000 */
240 tx = uref1;
241 ty = uref3;
242 t0 = pwm_reload - tx - ty;
243 if (t0 < 0) {
244 tx = ((float)tx / (tx + ty)) * pwm_reload;
245 ty = pwm_reload - tx;
246 t0 = 0;
247 }
248 tuon = t0 >> 1;
249 twon = ty + tuon;
250 tvon = tx + twon;
251 break;
252 case 6: /* sector6 000 001 101 111 101 001 000 */
253 tx = uref3;
254 ty = uref2;
255 t0 = pwm_reload - tx - ty;
256 if (t0 < 0) {
257 tx = ((float)tx / (tx + ty)) * pwm_reload;
258 ty = pwm_reload - tx;
259 t0 = 0;
260 }
261 tvon = t0 >> 1;
262 tuon = ty + tvon;
263 twon = tx + tuon;
264 break;
265
266 default:
267 tuon = (int)(pwm_reload/2);
268 tvon = (int)(pwm_reload/2);
269 twon = (int)(pwm_reload/2);
270 }
271 par->sector = sector;
272
273 if (tuon < 0)
274 tuon = 0;
275 if (tuon < 0)
276 tvon = 0;
277 if (tuon < 0)
278 twon = 0;
279
280 par->pwmout.pwm_u = tuon;
281 par->pwmout.pwm_v = tvon;
282 par->pwmout.pwm_w = twon;
283 }
284
hpm_mcl_bldc_foc_clarke(HPM_MOTOR_MATH_TYPE currentu,HPM_MOTOR_MATH_TYPE currentv,HPM_MOTOR_MATH_TYPE currentw,HPM_MOTOR_MATH_TYPE * currentalpha,HPM_MOTOR_MATH_TYPE * currentbeta)285 void hpm_mcl_bldc_foc_clarke(HPM_MOTOR_MATH_TYPE currentu, HPM_MOTOR_MATH_TYPE currentv, HPM_MOTOR_MATH_TYPE currentw,
286 HPM_MOTOR_MATH_TYPE *currentalpha, HPM_MOTOR_MATH_TYPE *currentbeta)
287 {
288 int32_t curbeta;
289
290 (void)currentw;
291 *currentalpha = currentu;
292 curbeta = (((int)(9370 * HPM_MOTOR_MATH_MDF_FL(currentu))) + ((int)(18918 * HPM_MOTOR_MATH_MDF_FL(currentv))))>>14;
293 *currentbeta = HPM_MOTOR_MATH_FL_MDF(curbeta);
294 }
295
hpm_mcl_bldc_foc_park(HPM_MOTOR_MATH_TYPE currentalpha,HPM_MOTOR_MATH_TYPE currentbeta,HPM_MOTOR_MATH_TYPE * currentd,HPM_MOTOR_MATH_TYPE * currentq,HPM_MOTOR_MATH_TYPE sin_angle,HPM_MOTOR_MATH_TYPE cos_angle)296 void hpm_mcl_bldc_foc_park(HPM_MOTOR_MATH_TYPE currentalpha, HPM_MOTOR_MATH_TYPE currentbeta,
297 HPM_MOTOR_MATH_TYPE *currentd, HPM_MOTOR_MATH_TYPE *currentq,
298 HPM_MOTOR_MATH_TYPE sin_angle, HPM_MOTOR_MATH_TYPE cos_angle)
299 {
300 *currentd = HPM_MOTOR_MATH_MUL(cos_angle, currentalpha) + HPM_MOTOR_MATH_MUL(sin_angle, currentbeta);
301 *currentq = HPM_MOTOR_MATH_MUL(-sin_angle, currentalpha) + HPM_MOTOR_MATH_MUL(cos_angle, currentbeta);
302 }
303
hpm_mcl_bldc_foc_current_cal(BLDC_CONTROL_CURRENT_PARA * par)304 void hpm_mcl_bldc_foc_current_cal(BLDC_CONTROL_CURRENT_PARA *par)
305 {
306 par->cal_u = HPM_MOTOR_MATH_FL_MDF(par->adc_u_middle - par->adc_u);
307 par->cal_v = HPM_MOTOR_MATH_FL_MDF(par->adc_v_middle - par->adc_v);
308 par->cal_w = HPM_MOTOR_MATH_FL_MDF(-(par->cal_u + par->cal_v));
309
310 }
311
hpm_mcl_bldc_foc_pi_contrl(BLDC_CONTRL_PID_PARA * par)312 void hpm_mcl_bldc_foc_pi_contrl(BLDC_CONTRL_PID_PARA *par)
313 {
314 HPM_MOTOR_MATH_TYPE result = 0;
315
316 HPM_MOTOR_MATH_TYPE curerr = 0;
317 HPM_MOTOR_MATH_TYPE portion_asp = 0;
318 HPM_MOTOR_MATH_TYPE portion_asi = 0;
319
320 curerr = par->target - par->cur;
321 portion_asp = HPM_MOTOR_MATH_MUL(curerr, (par->i_kp));
322 portion_asi = HPM_MOTOR_MATH_MUL(curerr, (par->i_ki)) + par->mem;
323 result = portion_asi + portion_asp;
324
325 if (result < (-par->i_max)) {
326 result = -par->i_max;
327 } else if (result > par->i_max) {
328 result = par->i_max;
329 } else {
330 par->mem = portion_asi;
331 }
332
333 par->outval = result;
334 }
335
hpm_mcl_bldc_foc_ctrl_dq_to_pwm(BLDC_CONTROL_FOC_PARA * par)336 void hpm_mcl_bldc_foc_ctrl_dq_to_pwm(BLDC_CONTROL_FOC_PARA *par)
337 {
338
339 HPM_MOTOR_MATH_TYPE sin_angle = 0;
340 HPM_MOTOR_MATH_TYPE cos_angle = 0;
341
342 par->samplcurpar.func_sampl(&par->samplcurpar);
343 hpm_mcl_bldc_foc_clarke(par->samplcurpar.cal_u, par->samplcurpar.cal_v, par->samplcurpar.cal_w,
344 &par->ialpha, &par->ibeta);
345
346 bldc_foc_sin_cos(par->electric_angle, PRECISION, &sin_angle, &cos_angle);
347 hpm_mcl_bldc_foc_park(par->ialpha, par->ibeta,
348 &par->currentdpipar.cur, &par->currentqpipar.cur,
349 sin_angle, cos_angle);
350
351 par->currentdpipar.func_pid(&par->currentdpipar);
352 par->currentqpipar.func_pid(&par->currentqpipar);
353
354 hpm_mcl_bldc_foc_inv_park(par->currentdpipar.outval, par->currentqpipar.outval,
355 &par->ualpha, &par->ubeta, sin_angle, cos_angle);
356 par->pwmpar.target_alpha = par->ualpha;
357 par->pwmpar.target_beta = par->ubeta;
358 par->pwmpar.func_spwm(&par->pwmpar);
359
360 }
361