1 /*
2 * Copyright (c) 2021 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #include "hpm_common.h"
8 #include "hpm_bldc_define.h"
9 #include "hpm_block.h"
10 /*HALL func begin*/
hpm_mcl_bldc_block_ctrl(uint8_t motorindex,uint8_t dir,uint8_t step)11 void hpm_mcl_bldc_block_ctrl(uint8_t motorindex, uint8_t dir, uint8_t step)
12 {
13 if (dir == BLDC_MOTOR_DIR_FORWARD) {
14 switch (step) {
15 case 2:
16 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
17 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
18 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
19 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
20 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
21 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
22 break;
23 case 3:
24 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
25 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
26 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
27 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
28 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
29 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
30 break;
31 case 4:
32 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
33 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
34 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
35 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
36 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
37 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
38 break;
39 case 5:
40 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
41 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
42 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
43 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
44 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
45 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
46 break;
47 case 6:
48 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
49 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
50 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
51 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
52 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
53 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
54 break;
55 case 1:
56 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
57 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
58 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
59 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
60 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
61 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
62 break;
63
64 default:
65 break;
66 }
67 } else {
68 switch (step) {
69 case 2:
70 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UL);/*uh*/
71 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*ul*/
72 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
73 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
74 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wh*/
75 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WH);/*wl*/
76 break;
77 case 3:
78 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UL);/*uh*/
79 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*ul*/
80 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vh*/
81 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VH);/*vl*/
82 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
83 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
84 break;
85 case 4:
86 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
87 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
88 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vh*/
89 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VH);/*vl*/
90 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WL);/*wh*/
91 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wl*/
92 break;
93 case 5:
94 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*uh*/
95 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UH);/*ul*/
96 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vh*/
97 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VL);/*vl*/
98 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WL);/*wh*/
99 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wl*/
100 break;
101 case 6:
102 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*uh*/
103 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_UH);/*ul*/
104 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VL);/*vh*/
105 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vl*/
106 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WH);/*wh*/
107 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wl*/
108 break;
109 case 1:
110 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UH);/*uh*/
111 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_UL);/*ul*/
112 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_VL);/*vh*/
113 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_VH);/*vl*/
114 hpm_mcl_bldc_pwm_disable(motorindex, BLDC_PWM_PIN_WL);/*wh*/
115 hpm_mcl_bldc_pwm_enable(motorindex, BLDC_PWM_PIN_WH);/*wl*/
116 break;
117
118 default:
119 break;
120 }
121 }
122 }
123 /*BLDC Block HALL position*/
hpm_mcl_bldc_block_step_get(bldc_hall_phase_t phase,uint8_t hall_u,uint8_t hall_v,uint8_t hall_w)124 uint8_t hpm_mcl_bldc_block_step_get(bldc_hall_phase_t phase, uint8_t hall_u, uint8_t hall_v, uint8_t hall_w)
125 {
126 /*
127 *tbl_para = (u<<2)|(v<<1)|(w);
128 * 0: resv;
129 */
130 const uint8_t hall_tbl_120[8] = {0, 4, 2, 3, 6, 5, 1, 0};
131 const uint8_t hall_tbl_60[8] = {5, 4, 0, 3, 6, 0, 1, 2};
132 if (phase == bldc_hall_phase_120) {
133 return hall_tbl_120[(hall_u<<2)|(hall_v<<1)|hall_w];
134 } else if (phase == bldc_hall_phase_60) {
135 return hall_tbl_60[(hall_u<<2)|(hall_v<<1)|hall_w];
136 }
137 return 0;
138 }
139
140 /*pi_ctrl*/
hpm_mcl_al_pi_ctrl_func(float * memory,float targetspeed,float speed,float kp,float ki,float max)141 float hpm_mcl_al_pi_ctrl_func(float *memory, float targetspeed, float speed, float kp, float ki, float max)
142 {
143 float result = 0;
144 float spderr = 0;
145 float portion_asp = 0;
146 float portion_asi = 0;
147 float *portion_asi_last_speed;
148
149 spderr = targetspeed - speed;
150 portion_asi_last_speed = memory;
151
152 portion_asp = spderr * kp;
153 portion_asi = spderr * ki;
154 portion_asi += *portion_asi_last_speed;
155 *portion_asi_last_speed = portion_asi;
156 if (*portion_asi_last_speed < (-max)) {
157 *portion_asi_last_speed = -max;
158 }
159 if (*portion_asi_last_speed > max) {
160 *portion_asi_last_speed = max;
161 }
162 result = portion_asi + portion_asp;
163 if (result < -max) {
164 result = -max;
165 }
166 if (result > max) {
167 result = max;
168 }
169
170 return result;
171 }
172