1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7
8 #include "hpm_hfi.h"
9
10 typedef enum {
11 pole_stat_init = 0,
12 pole_stat1 = 1,
13 pole_stat2,
14 pole_stat3,
15 pole_stat4,
16 pole_stat_ok
17 } pole_detect_stat_t;
18
hpm_mcl_hfi_sin_cos(HPM_MOTOR_MATH_TYPE angle,HPM_MOTOR_MATH_TYPE * sin_angle,HPM_MOTOR_MATH_TYPE * cos_angle)19 static void hpm_mcl_hfi_sin_cos(HPM_MOTOR_MATH_TYPE angle,
20 HPM_MOTOR_MATH_TYPE *sin_angle, HPM_MOTOR_MATH_TYPE *cos_angle)
21 {
22 float angle_f;
23
24 angle_f = angle;
25 *sin_angle = sinf(angle_f);
26 *cos_angle = cosf(angle_f);
27 }
28
hpm_mcl_hfi_core(float alpha,float beta,hpm_hfi_para_t * inject)29 void hpm_mcl_hfi_core(float alpha, float beta, hpm_hfi_para_t *inject)
30 {
31 float alpha_inject, beta_inject;
32
33 inject->e_alpha = (inject->last_alpha + alpha) / 2;
34 inject->e_beta = (inject->last_beta + beta) / 2;
35 alpha_inject = (alpha - inject->last_alpha);
36 beta_inject = (beta - inject->last_beta);
37
38 if (inject->vh < 0) {
39 alpha_inject = -alpha_inject;
40 beta_inject = -beta_inject;
41 }
42 inject->alpha = alpha_inject;
43 inject->beta = beta_inject;
44
45 inject->last_beta = beta;
46 inject->last_alpha = alpha;
47 }
48
hpm_mcl_hfi_pll(float alpha,float beta,float sin_val,float cos_val,hpm_hfi_pll_para_t * pll)49 float hpm_mcl_hfi_pll(float alpha, float beta, float sin_val, float cos_val, hpm_hfi_pll_para_t *pll)
50 {
51 float portion_asp = 0;
52 float portion_asi = 0;
53
54 pll->err_last = beta * cos_val - alpha * sin_val;
55 portion_asp = pll->kp * pll->err_last;
56 portion_asi = pll->ki * pll->err_last;
57 portion_asi += pll->mem;
58
59 if (portion_asi > pll->mem_max) {
60 portion_asi = pll->mem_max;
61 } else if (portion_asi < pll->mem_min) {
62 portion_asi = pll->mem_min;
63 }
64 pll->mem = portion_asi;
65 portion_asi += portion_asp;
66 pll->deta += portion_asi * pll->loop_s;
67 if (pll->deta > PI) {
68 pll->deta -= 2 * PI;
69 } else if (pll->deta < -PI) {
70 pll->deta += 2 * PI;
71 }
72 if (pll->deta < 0) {
73 pll->deta += 2 * PI;
74 }
75 pll->theta = pll->deta;
76
77 return pll->theta;
78 }
79
hpm_mcl_hfi_loop(BLDC_CONTROL_FOC_PARA * par,hpm_hfi_para_t * inject,hpm_hfi_pll_para_t * pll,hpm_hfi_pole_detect_para_t * pole)80 void hpm_mcl_hfi_loop(BLDC_CONTROL_FOC_PARA *par, hpm_hfi_para_t *inject,
81 hpm_hfi_pll_para_t *pll, hpm_hfi_pole_detect_para_t *pole)
82 {
83 static HPM_MOTOR_MATH_TYPE sin_angle = 0;
84 static HPM_MOTOR_MATH_TYPE cos_angle = 0;
85
86 par->samplcurpar.func_sampl(&par->samplcurpar);
87 hpm_mcl_bldc_foc_clarke(par->samplcurpar.cal_u, par->samplcurpar.cal_v, par->samplcurpar.cal_w,
88 &par->ialpha, &par->ibeta);
89 hpm_mcl_hfi_core(par->ialpha, par->ibeta, inject);
90 hpm_mcl_hfi_pll(inject->alpha, inject->beta, sin_angle, cos_angle, pll);
91 par->ialpha = inject->e_alpha;
92 par->ibeta = inject->e_beta;
93 hpm_mcl_hfi_sin_cos(par->electric_angle, &sin_angle, &cos_angle);
94 hpm_mcl_bldc_foc_park(par->ialpha, par->ibeta,
95 &par->currentdpipar.cur, &par->currentqpipar.cur,
96 sin_angle, cos_angle);
97 if (pole->currentd_force != 0) {
98 par->currentdpipar.target = pole->currentd_force;
99 }
100 par->currentdpipar.func_pid(&par->currentdpipar);
101 par->currentqpipar.func_pid(&par->currentqpipar);
102 par->currentdpipar.outval += inject->vh;
103 inject->vh = -inject->vh;
104 hpm_mcl_bldc_foc_inv_park(par->currentdpipar.outval, par->currentqpipar.outval,
105 &par->ualpha, &par->ubeta, sin_angle, cos_angle);
106 par->pwmpar.target_alpha = par->ualpha;
107 par->pwmpar.target_beta = par->ubeta;
108 par->pwmpar.func_spwm(&par->pwmpar);
109
110 }
111
hpm_mcl_hfi_pole_detect(BLDC_CONTROL_FOC_PARA * foc,hpm_hfi_para_t * inject,hpm_hfi_pll_para_t * pll,hpm_hfi_pole_detect_para_t * pole)112 bool hpm_mcl_hfi_pole_detect(BLDC_CONTROL_FOC_PARA *foc, hpm_hfi_para_t *inject,
113 hpm_hfi_pll_para_t *pll, hpm_hfi_pole_detect_para_t *pole)
114 {
115 float idh, iqh;
116 static float sin_angle, cos_angle;
117
118 if (pole->status == pole_stat_init) {
119 pole->currentd_force = 0;
120 foc->currentqpipar.target = 0;
121 foc->currentdpipar.target = 0;
122 pole->theta_zero = 0;
123 pole->theta_pi = 0;
124 } else if (pole->status == pole_stat1) {
125 pole->times++;
126 if (pole->times >= 50) {
127 pole->currentd_force = pole->current_d_init_val;
128 if (pole->times >= 55) {
129 hpm_mcl_hfi_sin_cos(foc->electric_angle, &sin_angle, &cos_angle);
130 hpm_mcl_bldc_foc_park(inject->alpha, inject->beta, &idh, &iqh, sin_angle, cos_angle);
131 pole->theta_zero += fabs(idh);
132 if (pole->times >= 60) {
133 pole->currentd_force = 0;
134 pole->times = 0;
135 pole->status = pole_stat2;
136 }
137 }
138 }
139 } else if (pole->status == pole_stat2) {
140 pole->times++;
141 if (pole->times > 50) {
142 pole->status = pole_stat3;
143 pole->times = 0;
144 }
145 } else if (pole->status == pole_stat3) {
146 pole->currentd_force = -pole->current_d_init_val;
147 pole->times++;
148 if (pole->times >= 5) {
149 hpm_mcl_hfi_sin_cos(foc->electric_angle, &sin_angle, &cos_angle);
150 hpm_mcl_bldc_foc_park(inject->alpha, inject->beta, &idh, &iqh, sin_angle, cos_angle);
151 pole->theta_pi += fabs(idh);
152 if (pole->times >= 10) {
153 pole->currentd_force = 0;
154 pole->times = 0;
155 pole->status = pole_stat4;
156 if (pole->theta_zero > pole->theta_pi) {
157 pll->deta += PI;
158 }
159 if (pll->deta > PI) {
160 pll->deta -= 2 * PI;
161 } else if (pll->deta < -PI) {
162 pll->deta += 2 * PI;
163 }
164
165 if (pll->deta < 0) {
166 pll->deta += 2 * PI;
167 }
168 }
169 }
170 } else if (pole->status == pole_stat4) {
171 pole->times++;
172 if (pole->times > 500) {
173 pole->status = pole_stat_ok;
174 pole->times = 0;
175 pole->theta_zero = 0;
176 pole->theta_pi = 0;
177 }
178 } else if (pole->status == pole_stat_ok) {
179 return true;
180 }
181 return false;
182 }
183