1 /*
2 * Copyright (c) 2021-2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #include "hpm_common.h"
8 #include <stdlib.h>
9 #include "hpm_bldc_define.h"
10 #include "hpm_smc.h"
11 #include "hpm_foc.h"
12 #include <math.h>
13
14 #define PI (3.1415926)
15
hpm_mcl_sin_cos(float angle,float * sin_angle,float * cos_angle)16 static void hpm_mcl_sin_cos(float angle, float *sin_angle, float *cos_angle)
17 {
18 float angle_f;
19
20 angle_f = angle * PI / 180;
21 *sin_angle = sinf(angle_f);
22 *cos_angle = cosf(angle_f);
23 }
24
hpm_mcl_smc_pos_cal(hpm_mcl_para_t * par)25 void hpm_mcl_smc_pos_cal(hpm_mcl_para_t *par)
26 {
27 float alhpa_err;
28 float beta_err;
29
30 par->ialpha_mem = par->i_motorpar->o_smc_f * par->ialpha_mem +
31 par->i_motorpar->o_smc_g * (*par->ualpha - par->alpha_cal - par->zalpha_cal);
32 par->ibeta_mem = par->i_motorpar->o_smc_f * par->ibeta_mem +
33 par->i_motorpar->o_smc_g * (*par->ubeta - par->beta_cal - par->zbeta_cal);
34
35 alhpa_err = par->ialpha_mem - *par->ialpha;
36 beta_err = par->ibeta_mem - *par->ibeta;
37
38 if (fabs(alhpa_err) < par->zero)
39 par->zalpha_cal = 2 * alhpa_err * par->ksmc;
40 else if (alhpa_err >= par->zero)
41 par->zalpha_cal = par->ksmc;
42 else if (alhpa_err <= -par->zero)
43 par->zalpha_cal = -par->ksmc;
44
45 if (fabs(beta_err) < par->zero)
46 par->zbeta_cal = 2 * beta_err * par->ksmc;
47 else if (beta_err >= par->zero)
48 par->zbeta_cal = par->ksmc;
49 else if (beta_err <= -par->zero)
50 par->zbeta_cal = -par->ksmc;
51
52 /**
53 * @brief LOW-PASS
54 *
55 */
56 par->alpha_cal = (1 - par->filter_coeff) * par->alpha_cal + par->filter_coeff * par->zalpha_cal;
57 par->beta_cal = (1 - par->filter_coeff) * par->beta_cal + par->filter_coeff * par->zbeta_cal;
58 }
59
hpm_mcl_smc_const_cal(hpm_motor_para_t * par)60 void hpm_mcl_smc_const_cal(hpm_motor_para_t *par)
61 {
62 par->o_smc_g = par->i_samplingper_s / par->i_lstator_h;
63 par->o_smc_f = 1 - (par->i_rstator_ohm / par->i_lstator_h) * par->i_samplingper_s;
64 }
65
hpm_mcl_smc_pll(hpm_mcl_para_t * par,hpm_smc_pll_para_t * pll)66 float hpm_mcl_smc_pll(hpm_mcl_para_t *par, hpm_smc_pll_para_t *pll)
67 {
68 float portion_asp = 0;
69 float portion_asi = 0;
70
71 pll->err = -par->alpha_cal * cosf(pll->theta_last) -
72 par->beta_cal * sinf(pll->theta_last);
73 /**
74 * @brief PI
75 *
76 */
77 portion_asp = pll->kp * pll->err;
78 portion_asi = pll->ki * pll->err;
79 portion_asi += pll->mem;
80
81 if (portion_asi > pll->max_i) {
82 portion_asi = pll->max_i;
83 } else if (portion_asi < pll->min_i) {
84 portion_asi = pll->min_i;
85 }
86 pll->mem = portion_asi;
87 pll->speedout = portion_asi + portion_asp;
88
89 if (pll->speedout > pll->max_o) {
90 pll->speedout = pll->max_o;
91 } else if (pll->speedout < pll->min_o) {
92 pll->speedout = pll->min_o;
93 }
94
95 pll->theta_last += pll->speedout * pll->loop_in_sec;
96 pll->theta_last = fmodf(pll->theta_last, PI*2);
97 pll->theta = fmodf(pll->theta_last * 57.29578 + pll->theta0, 360);
98
99 return pll->theta;
100 }
101
hpm_mcl_smc_loop(BLDC_CONTROL_FOC_PARA * par,hpm_mcl_para_t * smc,hpm_smc_pll_para_t * pll,uint8_t * is_smc_enable)102 void hpm_mcl_smc_loop(BLDC_CONTROL_FOC_PARA *par, hpm_mcl_para_t *smc, hpm_smc_pll_para_t *pll, uint8_t *is_smc_enable)
103 {
104
105 float sin_angle = 0;
106 float cos_angle = 0;
107
108 par->samplcurpar.func_sampl(&par->samplcurpar);
109 hpm_mcl_bldc_foc_clarke(par->samplcurpar.cal_u, par->samplcurpar.cal_v, par->samplcurpar.cal_w,
110 &par->ialpha, &par->ibeta);
111 if (par->pos_estimator_par.func != NULL) {
112 par->pos_estimator_par.func(par->pos_estimator_par.par);
113 hpm_mcl_smc_pll(smc, pll);
114 if (*is_smc_enable) {
115 par->electric_angle = pll->theta;
116 }
117 }
118 hpm_mcl_sin_cos(par->electric_angle, &sin_angle, &cos_angle);
119 hpm_mcl_bldc_foc_park(par->ialpha, par->ibeta,
120 &par->currentdpipar.cur, &par->currentqpipar.cur,
121 sin_angle, cos_angle);
122
123 par->currentdpipar.func_pid(&par->currentdpipar);
124 par->currentqpipar.func_pid(&par->currentqpipar);
125
126 hpm_mcl_bldc_foc_inv_park(par->currentdpipar.outval, par->currentqpipar.outval,
127 &par->ualpha, &par->ubeta, sin_angle, cos_angle);
128 par->pwmpar.target_alpha = par->ualpha;
129 par->pwmpar.target_beta = par->ubeta;
130 par->pwmpar.func_spwm(&par->pwmpar);
131
132 }
133