1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_mcl_control.h"
9 #include "hpm_mcl_math.h"
10 #include "math.h"
11
12 #define SQRT3 (1.7320508075688773f) /**< sqrt(3) */
13 #define SQRT3_DIV3 (0.5773502691896258f) /**< sqrt(3)/3 */
14
hpm_mcl_control_sin(float x)15 float hpm_mcl_control_sin(float x)
16 {
17 return sinf(x);
18 }
19
hpm_mcl_control_cos(float x)20 float hpm_mcl_control_cos(float x)
21 {
22 return cosf(x);
23 }
24
hpm_mcl_control_arctan(float y,float x)25 float hpm_mcl_control_arctan(float y, float x)
26 {
27 return atan2(y, x);
28 }
29
hpm_mcl_control_clarke(float ia,float ib,float ic,float * alpha,float * beta)30 hpm_mcl_stat_t hpm_mcl_control_clarke(float ia, float ib, float ic,
31 float *alpha, float *beta)
32 {
33 (void)ic;
34 MCL_ASSERT_OPT(alpha != NULL, mcl_invalid_pointer);
35 MCL_ASSERT_OPT(beta != NULL, mcl_invalid_pointer);
36 *alpha = ia;
37 *beta = SQRT3_DIV3 * ia + (SQRT3_DIV3 * 2) * ib;
38
39 return mcl_success;
40 }
41
hpm_mcl_control_park(float alpha,float beta,float sin_x,float cos_x,float * d,float * q)42 hpm_mcl_stat_t hpm_mcl_control_park(float alpha, float beta, float sin_x, float cos_x,
43 float *d, float *q)
44 {
45 MCL_ASSERT_OPT(d != NULL, mcl_invalid_pointer);
46 MCL_ASSERT_OPT(q != NULL, mcl_invalid_pointer);
47 *d = cos_x * alpha + sin_x * beta;
48 *q = -sin_x * alpha + cos_x *beta;
49
50 return mcl_success;
51 }
52
hpm_mcl_control_pi(float ref,float sens,mcl_control_pid_t * pid_x,float * output)53 hpm_mcl_stat_t hpm_mcl_control_pi(float ref, float sens, mcl_control_pid_t *pid_x, float *output)
54 {
55 float err;
56 float val;
57
58 MCL_ASSERT_OPT(output != NULL, mcl_invalid_pointer);
59 err = ref - sens;
60 pid_x->integral += pid_x->cfg.ki * err;
61 MCL_VALUE_LIMIT(pid_x->integral, pid_x->cfg.integral_min, pid_x->cfg.integral_max);
62 val = pid_x->cfg.kp * err + pid_x->integral;
63 MCL_VALUE_LIMIT(val, pid_x->cfg.output_min, pid_x->cfg.output_max);
64 *output = val;
65
66 return mcl_success;
67 }
68
hpm_mcl_control_inv_park(float d,float q,float sin_x,float cos_x,float * alpha,float * beta)69 hpm_mcl_stat_t hpm_mcl_control_inv_park(float d, float q, float sin_x,
70 float cos_x, float *alpha, float *beta)
71 {
72 MCL_ASSERT_OPT(alpha != NULL, mcl_invalid_pointer);
73 MCL_ASSERT_OPT(beta != NULL, mcl_invalid_pointer);
74 *alpha = cos_x * d - sin_x * q;
75 *beta = sin_x * d + cos_x * q;
76
77 return mcl_success;
78 }
79
hpm_mcl_control_svpwm(float alpha,float beta,float vbus,mcl_control_svpwm_duty_t * duty)80 hpm_mcl_stat_t hpm_mcl_control_svpwm(float alpha, float beta, float vbus, mcl_control_svpwm_duty_t *duty)
81 {
82 float val0, val1;
83 float val2, val3;
84 float val4, val5, val6;
85 mcl_svpwm_sector_t sector;
86
87 MCL_ASSERT_OPT(duty != NULL, mcl_invalid_pointer);
88 MCL_ASSERT(vbus != 0, mcl_invalid_argument);
89 vbus = 1 / (vbus * 2 / 3);
90 alpha = alpha * vbus;
91 beta = beta * vbus;
92 val0 = beta / alpha;
93 val1 = alpha / beta;
94
95 if (beta > 0) {
96 if (alpha > 0) {
97 if (val0 < SQRT3) {
98 sector = svpwm_sector1;
99 } else {
100 sector = svpwm_sector2;
101 }
102 } else if (val1 > -SQRT3_DIV3) {
103 sector = svpwm_sector2;
104 } else {
105 sector = svpwm_sector3;
106 }
107 } else if (alpha > 0) {
108 if (val0 < -SQRT3) {
109 sector = svpwm_sector5;
110 } else {
111 sector = svpwm_sector6;
112 }
113 } else if (val0 < SQRT3) {
114 sector = svpwm_sector4;
115 } else {
116 sector = svpwm_sector5;
117 }
118 val2 = SQRT3_DIV3 * beta;
119 val3 = SQRT3_DIV3 * 2 * beta;
120
121
122 switch (sector) {
123 case svpwm_sector1:
124 val4 = (alpha - val2);
125 val5 = (1 - val4 - val3) / 2;
126 val4 += val5;
127 val6 = val4 + val3;
128 duty->a = val5;
129 duty->b = val4;
130 duty->c = val6;
131 break;
132 case svpwm_sector2:
133 val4 = alpha + val2;
134 val2 = val2 - alpha;
135 val5 = (1 - val4 - val2) / 2;
136 val2 += val5;
137 val6 = val2 + val4;
138 duty->a = val2;
139 duty->b = val5;
140 duty->c = val6;
141 break;
142 case svpwm_sector3:
143 val4 = -alpha - val2;
144 val5 = (1 - val4 - val3) / 2;
145 val3 += val5;
146 val6 = val3 + val4;
147 duty->a = val6;
148 duty->b = val5;
149 duty->c = val3;
150 break;
151 case svpwm_sector4:
152 val4 = val2 - alpha;
153 val5 = (1 - val4 + val3) / 2;
154 val3 = val5 - val3;
155 val6 = val3 + val4;
156 duty->a = val6;
157 duty->b = val3;
158 duty->c = val5;
159 break;
160 case svpwm_sector5:
161 val4 = alpha - val2;
162 val2 = -val2 - alpha;
163 val5 = (1 - val4 - val2) / 2;
164 val2 += val5;
165 val6 = val2 + val4;
166 duty->a = val2;
167 duty->b = val6;
168 duty->c = val5;
169 break;
170 case svpwm_sector6:
171 val4 = val2 + alpha;
172 val5 = (1 - val4 + val3) / 2;
173 val4 += val5;
174 val6 = val4 - val3;
175 duty->a = val5;
176 duty->b = val6;
177 duty->c = val4;
178 break;
179
180 default:
181 duty->a = 0;
182 duty->b = 0;
183 duty->c = 0;
184 break;
185 }
186
187 return mcl_success;
188 }
189
hpm_mcl_control_step_svpwm(float alpha,float beta,float vbus,mcl_control_svpwm_duty_t * duty)190 hpm_mcl_stat_t hpm_mcl_control_step_svpwm(float alpha, float beta, float vbus, mcl_control_svpwm_duty_t *duty)
191 {
192 float ta0on = 0, ta1on = 0, tb0on = 0, tb1on = 0;
193
194 MCL_ASSERT_OPT(duty != NULL, mcl_invalid_pointer);
195 MCL_ASSERT(vbus != 0, mcl_invalid_argument);
196 vbus = 1 / (vbus * 2 / 3);
197 alpha = alpha * vbus;
198 beta = beta * vbus;
199
200 if (alpha > 0) {
201 ta1on = alpha;
202 } else {
203 ta0on = -alpha;
204 }
205 if (beta > 0) {
206 tb1on = beta;
207 } else {
208 tb0on = -beta;
209 }
210
211 MCL_VALUE_LIMIT(ta0on, 0, 1);
212 MCL_VALUE_LIMIT(ta1on, 0, 1);
213 MCL_VALUE_LIMIT(tb0on, 0, 1);
214 MCL_VALUE_LIMIT(tb1on, 0, 1);
215
216 duty->a0 = ta0on;
217 duty->a1 = ta1on;
218 duty->b0 = tb0on;
219 duty->b1 = tb1on;
220
221 return mcl_success;
222 }
223
hpm_mcl_control_init(mcl_control_t * control,mcl_control_cfg_t * cfg)224 hpm_mcl_stat_t hpm_mcl_control_init(mcl_control_t *control, mcl_control_cfg_t *cfg)
225 {
226 MCL_ASSERT(control != NULL, mcl_invalid_pointer);
227 MCL_ASSERT(cfg != NULL, mcl_invalid_pointer);
228
229 /**
230 * @brief Data initialisation
231 *
232 */
233 control->cfg = cfg;
234 control->cfg->currentd_pid_cfg.integral = 0;
235 control->cfg->currentq_pid_cfg.integral = 0;
236 control->cfg->position_pid_cfg.integral = 0;
237 control->cfg->speed_pid_cfg.integral = 0;
238
239 /**
240 * @brief function initialisation
241 *
242 */
243 control->method.arctan_x = &hpm_mcl_control_arctan;
244 control->method.clarke = &hpm_mcl_control_clarke;
245 control->method.cos_x = &hpm_mcl_control_cos;
246 control->method.currentd_pid = &hpm_mcl_control_pi;
247 control->method.currentq_pid = &hpm_mcl_control_pi;
248 control->method.invpark = &hpm_mcl_control_inv_park;
249 control->method.park = &hpm_mcl_control_park;
250 control->method.position_pid = &hpm_mcl_control_pi;
251 control->method.sin_x = &hpm_mcl_control_sin;
252 control->method.speed_pid = &hpm_mcl_control_pi;
253 control->method.svpwm = &hpm_mcl_control_svpwm;
254 control->method.step_svpwm = &hpm_mcl_control_step_svpwm;
255 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.arctan_x, control->cfg->callback.method.arctan_x);
256 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.clarke, control->cfg->callback.method.clarke);
257 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.cos_x, control->cfg->callback.method.cos_x);
258 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.currentd_pid, control->cfg->callback.method.currentd_pid);
259 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.invpark, control->cfg->callback.method.invpark);
260 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.park, control->cfg->callback.method.park);
261 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.position_pid, control->cfg->callback.method.position_pid);
262 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.sin_x, control->cfg->callback.method.sin_x);
263 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.speed_pid, control->cfg->callback.method.speed_pid);
264 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.svpwm, control->cfg->callback.method.svpwm);
265 MCL_FUNCTION_INIT_IF_NO_EMPTY(control->method.svpwm, control->cfg->callback.method.step_svpwm);
266
267 control->cfg->callback.init();
268
269 return mcl_success;
270 }
271