1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #include "hpm_mcl_loop.h"
8
hpm_mcl_loop_init(mcl_loop_t * loop,mcl_loop_cfg_t * cfg,mcl_cfg_t * mcl_cfg,mcl_encoder_t * encoder,mcl_analog_t * analog,mcl_control_t * control,mcl_drivers_t * drivers,mcl_path_plan_t * path)9 hpm_mcl_stat_t hpm_mcl_loop_init(mcl_loop_t *loop, mcl_loop_cfg_t *cfg, mcl_cfg_t *mcl_cfg,
10 mcl_encoder_t *encoder, mcl_analog_t *analog,
11 mcl_control_t *control, mcl_drivers_t *drivers, mcl_path_plan_t *path)
12 {
13 MCL_ASSERT(loop != NULL, mcl_invalid_pointer);
14 MCL_ASSERT(cfg != NULL, mcl_invalid_pointer);
15 MCL_ASSERT(mcl_cfg != NULL, mcl_invalid_pointer);
16 if (cfg->mode != mcl_mode_step_foc) {
17 MCL_ASSERT(encoder != NULL, mcl_invalid_pointer);
18 } else {
19 MCL_ASSERT(path != NULL, mcl_invalid_pointer);
20 }
21 MCL_ASSERT(analog != NULL, mcl_invalid_pointer);
22 MCL_ASSERT(control != NULL, mcl_invalid_pointer);
23 MCL_ASSERT(drivers != NULL, mcl_invalid_pointer);
24
25 loop->enable = false;
26 loop->cfg = cfg;
27 loop->path = path;
28 loop->status = loop_status_run;
29 #ifndef HPM_MCL_Q_EN
30 loop->const_vbus = &mcl_cfg->physical.motor.vbus;
31 #else
32 loop->const_vbus = &mcl_cfg->physical_q.motor.vbus;
33 #endif
34 loop->const_time.current_ts = &mcl_cfg->physical.time.current_loop_ts;
35 loop->const_time.position_ts = &mcl_cfg->physical.time.speed_loop_ts;
36 loop->const_time.speed_ts = &mcl_cfg->physical.time.speed_loop_ts;
37 loop->encoder = encoder;
38 loop->analog = analog;
39 loop->control = control;
40 loop->drivers = drivers;
41 loop->time.position_ts = 0;
42 loop->time.speed_ts = 0;
43 loop->exec_ref.id = 0;
44 loop->exec_ref.iq = 0;
45 loop->exec_ref.position = 0;
46 loop->exec_ref.speed = 0;
47
48 hpm_mcl_loop_disable_all_user_set_value(loop);
49
50 return mcl_success;
51 }
52
hpm_mcl_loop_set_current_d(mcl_loop_t * loop,mcl_user_value_t id)53 hpm_mcl_stat_t hpm_mcl_loop_set_current_d(mcl_loop_t *loop, mcl_user_value_t id)
54 {
55 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
56 loop->ref_id = id;
57 return mcl_success;
58 }
59
hpm_mcl_loop_set_current_q(mcl_loop_t * loop,mcl_user_value_t iq)60 hpm_mcl_stat_t hpm_mcl_loop_set_current_q(mcl_loop_t *loop, mcl_user_value_t iq)
61 {
62 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
63 loop->ref_iq = iq;
64 return mcl_success;
65 }
66
hpm_mcl_loop_set_speed(mcl_loop_t * loop,mcl_user_value_t speed)67 hpm_mcl_stat_t hpm_mcl_loop_set_speed(mcl_loop_t *loop, mcl_user_value_t speed)
68 {
69 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
70 loop->ref_speed = speed;
71 return mcl_success;
72 }
73
hpm_mcl_loop_set_position(mcl_loop_t * loop,mcl_user_value_t position)74 hpm_mcl_stat_t hpm_mcl_loop_set_position(mcl_loop_t *loop, mcl_user_value_t position)
75 {
76 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
77 loop->ref_position = position;
78 return mcl_success;
79 }
80
hpm_mcl_loop_disable_all_user_set_value(mcl_loop_t * loop)81 hpm_mcl_stat_t hpm_mcl_loop_disable_all_user_set_value(mcl_loop_t *loop)
82 {
83 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
84 loop->ref_id.enable = false;
85 loop->ref_iq.enable = false;
86 loop->ref_position.enable = false;
87 loop->ref_speed.enable = false;
88 return mcl_success;
89 }
90
91
hpm_mcl_bldc_loop(mcl_loop_t * loop)92 hpm_mcl_stat_t hpm_mcl_bldc_loop(mcl_loop_t *loop)
93 {
94 float ref_speed = 0;
95 float ref_position = 0;
96 float ia, ib, ic;
97 float theta;
98 float alpha, beta;
99 float sinx, cosx;
100 float sens_d, sens_q;
101 float ref_d = 0, ref_q = 0;
102 float ud, uq;
103 float theta_abs;
104 mcl_control_svpwm_duty_t duty;
105 #if defined(MCL_CFG_EN_THETA_FORECAST) && MCL_CFG_EN_THETA_FORECAST
106 float sinx_, cosx_;
107 float theta_forecast;
108 #endif
109
110 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
111 theta = hpm_mcl_encoder_get_theta(loop->encoder);
112 #if defined(MCL_CFG_EN_THETA_FORECAST) && MCL_CFG_EN_THETA_FORECAST
113 theta_forecast = hpm_mcl_encoder_get_forecast_theta(loop->encoder);
114 #endif
115 if (loop->enable) {
116 if (loop->cfg->enable_position_loop) {
117 loop->time.position_ts += *loop->const_time.current_ts;
118 MCL_FUNCTION_SET_IF_ELSE_TRUE(loop->ref_position.enable, ref_position, loop->ref_position.value, loop->exec_ref.position);
119 if (loop->time.position_ts >= *loop->const_time.position_ts) {
120 loop->time.position_ts = 0;
121 MCL_ASSERT_EXEC_CODE_AND_RETURN(hpm_mcl_encoder_get_absolute_theta(loop->encoder, &theta_abs) == mcl_success,
122 loop->status = loop_status_fail, mcl_fail);
123 loop->control->method.position_pid(ref_position, theta_abs, &loop->control->cfg->position_pid_cfg, &loop->exec_ref.speed);
124 }
125 } else {
126 loop->exec_ref.speed = 0;
127 loop->time.position_ts = 0;
128 }
129 if (loop->cfg->enable_speed_loop) {
130 loop->time.speed_ts += *loop->const_time.current_ts;
131 MCL_FUNCTION_SET_IF_ELSE_TRUE(loop->ref_speed.enable, ref_speed, loop->ref_speed.value, loop->exec_ref.speed);
132 if (loop->time.speed_ts >= *loop->const_time.speed_ts) {
133 loop->time.speed_ts = 0;
134 loop->control->method.speed_pid(ref_speed, hpm_mcl_encoder_get_speed(loop->encoder),
135 &loop->control->cfg->speed_pid_cfg, &loop->exec_ref.iq);
136 }
137 } else {
138 loop->time.speed_ts = 0;
139 loop->exec_ref.iq = 0;
140 }
141 switch (loop->cfg->mode) {
142 case mcl_mode_foc:
143 /**
144 * @brief current sample
145 *
146 */
147 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_get_value(loop->analog, analog_a_current, &ia), loop->status, loop_status_fail);
148 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_get_value(loop->analog, analog_b_current, &ib), loop->status, loop_status_fail);
149 ic = -(ia + ib);
150
151 MCL_VALUE_SET_IF_TRUE(loop->ref_id.enable, ref_d, loop->ref_id.value);
152 MCL_FUNCTION_SET_IF_ELSE_TRUE(loop->ref_iq.enable, ref_q, loop->ref_iq.value, loop->exec_ref.iq);
153 loop->control->method.clarke(ia, ib, ic, &alpha, &beta);
154 sinx = loop->control->method.sin_x(theta);
155 cosx = loop->control->method.cos_x(theta);
156 loop->control->method.park(alpha, beta, sinx, cosx, &sens_d, &sens_q);
157 loop->control->method.currentd_pid(ref_d, sens_d, &loop->control->cfg->currentd_pid_cfg, &ud);
158 loop->control->method.currentq_pid(ref_q, sens_q, &loop->control->cfg->currentq_pid_cfg, &uq);
159 #if defined(MCL_CFG_EN_THETA_FORECAST) && MCL_CFG_EN_THETA_FORECAST
160 sinx_ = loop->control->method.sin_x(theta_forecast);
161 cosx_ = loop->control->method.cos_x(theta_forecast);
162 loop->control->method.invpark(ud, uq, sinx_, cosx_, &alpha, &beta);
163 #else
164 loop->control->method.invpark(ud, uq, sinx, cosx, &alpha, &beta);
165 #endif
166 loop->control->method.svpwm(alpha, beta, *loop->const_vbus, &duty);
167 hpm_mcl_drivers_update_bldc_duty(loop->drivers, duty.a, duty.b, duty.c);
168
169 break;
170 case mcl_mode_hardware_foc:
171 MCL_ASSERT_EXEC_CODE_AND_RETURN(false, loop->status = loop_status_fail, mcl_in_development);
172 break;
173 default:
174 break;
175 }
176 } else {
177 duty.a = 0;
178 duty.b = 0;
179 duty.c = 0;
180 hpm_mcl_drivers_update_bldc_duty(loop->drivers, duty.a, duty.b, duty.c);
181 }
182 return mcl_success;
183 }
184
hpm_mcl_step_loop(mcl_loop_t * loop)185 hpm_mcl_stat_t hpm_mcl_step_loop(mcl_loop_t *loop)
186 {
187 float ia, ib;
188 float theta;
189 float alpha, beta;
190 float sinx, cosx;
191 float sens_d, sens_q;
192 float ref_d = 0;
193 float ud, uq;
194 float sinx_, cosx_;
195 float theta_;
196 mcl_control_svpwm_duty_t duty;
197
198 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
199 theta = hpm_mcl_path_get_current_theta(loop->path);
200 theta_ = MCL_ANGLE_MOD_X(0, MCL_PI * 2, (hpm_mcl_path_get_next_theta(loop->path) - (0.25 * MCL_PI)));
201
202 if (loop->enable) {
203 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_get_value(loop->analog, analog_a_current, &ia), loop->status, loop_status_fail);
204 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_get_value(loop->analog, analog_b_current, &ib), loop->status, loop_status_fail);
205 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_step_convert(loop->analog, ia, analog_a_current, theta, &ia), loop->status, loop_status_fail);
206 MCL_STATUS_SET_IF_TRUE(mcl_success != hpm_mcl_analog_step_convert(loop->analog, ib, analog_b_current, theta, &ib), loop->status, loop_status_fail);
207
208 theta = MCL_ANGLE_MOD_X(0, MCL_PI * 2, (theta - (0.25 * MCL_PI)));
209 MCL_VALUE_SET_IF_TRUE(loop->ref_id.enable, ref_d, loop->ref_id.value);
210 alpha = ia;
211 beta = ib;
212 sinx = loop->control->method.sin_x(theta);
213 cosx = loop->control->method.cos_x(theta);
214 loop->control->method.park(alpha, beta, sinx, cosx, &sens_d, &sens_q);
215 loop->control->method.currentd_pid(ref_d, sens_d, &loop->control->cfg->currentd_pid_cfg, &ud);
216 loop->control->method.currentq_pid(0, sens_q, &loop->control->cfg->currentq_pid_cfg, &uq);
217 sinx_ = loop->control->method.sin_x(theta_);
218 cosx_ = loop->control->method.cos_x(theta_);
219 loop->control->method.invpark(ud, uq, sinx_, cosx_, &alpha, &beta);
220 loop->control->method.step_svpwm(alpha, beta, *loop->const_vbus, &duty);
221 hpm_mcl_drivers_update_step_duty(loop->drivers, duty.a0, duty.a1, duty.b0, duty.b1);
222 } else {
223 duty.a0 = 0;
224 duty.a1 = 0;
225 duty.b0 = 0;
226 duty.b1 = 0;
227 hpm_mcl_drivers_update_step_duty(loop->drivers, duty.a0, duty.a1, duty.b0, duty.b1);
228 }
229 return mcl_success;
230 }
231
hpm_mcl_loop(mcl_loop_t * loop)232 hpm_mcl_stat_t hpm_mcl_loop(mcl_loop_t *loop)
233 {
234 MCL_ASSERT_OPT(loop != NULL, mcl_invalid_pointer);
235 switch (loop->cfg->mode) {
236 case mcl_mode_foc:
237 case mcl_mode_hardware_foc:
238 return hpm_mcl_bldc_loop(loop);
239 case mcl_mode_block:
240 MCL_ASSERT_EXEC_CODE_AND_RETURN(false, loop->status = loop_status_fail, mcl_in_development);
241 break;
242 case mcl_mode_step_foc:
243 return hpm_mcl_step_loop(loop);
244 break;
245 default:
246 MCL_ASSERT_EXEC_CODE_AND_RETURN(false, loop->status = loop_status_fail, mcl_fail);
247 break;
248 }
249 return mcl_fail;
250 }
251