1 /*
2 * Copyright (c) 2023 HPMicro
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 *
6 */
7 #include <math.h>
8 #include "hpm_mcl_encoder.h"
9 #include "hpm_mcl_physical.h"
10
hpm_mcl_encoder_init(mcl_encoder_t * encoder,mcl_cfg_t * mcl_cfg,mcl_encoer_cfg_t * encoder_cfg,mcl_filter_iir_df1_t * iir)11 hpm_mcl_stat_t hpm_mcl_encoder_init(mcl_encoder_t *encoder, mcl_cfg_t *mcl_cfg, mcl_encoer_cfg_t *encoder_cfg, mcl_filter_iir_df1_t *iir)
12 {
13 MCL_ASSERT(encoder != NULL, mcl_invalid_pointer);
14 encoder->status = encoder_status_null;
15 /**
16 * @brief parameter check
17 *
18 */
19 MCL_ASSERT(mcl_cfg != NULL, mcl_invalid_pointer);
20 MCL_ASSERT(encoder_cfg != NULL, mcl_invalid_pointer);
21 MCL_ASSERT(iir != NULL, mcl_invalid_pointer);
22 MCL_ASSERT((encoder_cfg->callback.start_sample != NULL), mcl_invalid_pointer);
23 MCL_ASSERT(((encoder_cfg->disable_start_sample_interrupt == true) || (encoder_cfg->callback.get_lead_tick != NULL)), mcl_invalid_pointer);
24 MCL_ASSERT(((encoder_cfg->disable_start_sample_interrupt == true) || (encoder_cfg->callback.update_trig_lead_tick != NULL)), mcl_invalid_pointer);
25 MCL_ASSERT((encoder_cfg->callback.init != NULL), mcl_invalid_pointer);
26 MCL_ASSERT((encoder_cfg->callback.get_theta != NULL), mcl_invalid_pointer);
27 MCL_ASSERT((encoder_cfg->speed_cal_method != encoder_method_user) || (encoder_cfg->callback.process_by_user != NULL), mcl_invalid_argument);
28 /**
29 * @brief null function initialisation
30 *
31 */
32 memset(encoder->cal_speed.memory, 0, MCL_ENCODER_CAL_STRUCT_MAX_MEMMORY * sizeof(uint32_t));
33 encoder->mcu_clock_tick = &mcl_cfg->physical.time.mcu_clock_tick;
34 encoder->cal_speed.pll_method.cfg = (mcl_encoder_cal_speed_pll_cfg_t *)&encoder_cfg->cal_speed_pll_cfg;
35 encoder->cal_speed.pll_method.period_call_time_s = (float *)&encoder_cfg->period_call_time_s;
36 encoder->iirfilter = iir;
37 encoder->current_loop_ts = &mcl_cfg->physical.time.current_loop_ts;
38 encoder->pole_num = &mcl_cfg->physical.motor.pole_num;
39 /**
40 * @brief parameter initial
41 *
42 */
43 encoder->cfg = encoder_cfg;
44 encoder->result.speed = 0;
45 encoder->result.theta = 0;
46 encoder->result.theta_forecast = 0;
47 memset(iir->mem, 0, iir->cfg->section * sizeof(mcl_filter_iir_df1_memory_t));
48 encoder->force_set_theta.enable = false;
49 encoder->force_set_theta.value = 0;
50
51 encoder->status = encoder_status_init;
52 encoder->cfg->callback.init();
53 MCL_ASSERT(encoder->cfg->callback.start_sample() == mcl_success, mcl_encoder_start_sample_error);
54 mcl_user_delay_us(encoder->cfg->communication_interval_us * 2);
55 MCL_ASSERT((encoder->cfg->callback.get_theta(&encoder->theta_initial) == mcl_success), mcl_encoder_get_theta_error);
56 encoder->status = encoder_status_run;
57
58 return mcl_success;
59 }
60
hpm_mcl_encoder_start_sample(mcl_encoder_t * encoder)61 hpm_mcl_stat_t hpm_mcl_encoder_start_sample(mcl_encoder_t *encoder)
62 {
63 uint32_t tick;
64
65 MCL_ASSERT_EXEC_CODE_AND_RETURN_OPT(encoder != NULL, encoder->status = encoder_status_fail, mcl_invalid_pointer);
66 MCL_ASSERT_EXEC_CODE_AND_RETURN(encoder->cfg->callback.start_sample() == mcl_success,
67 encoder->status = encoder_status_fail, mcl_encoder_start_sample_error);
68 MCL_ASSERT_EXEC_CODE_AND_RETURN(encoder->cfg->callback.get_lead_tick(&tick) == mcl_success,
69 encoder->status = encoder_status_fail, mcl_encoder_get_tick_error);
70 MCL_ASSERT_EXEC_CODE_AND_RETURN(encoder->cfg->callback.update_trig_lead_tick(tick) == mcl_success,
71 encoder->status = encoder_status_fail, mcl_encoder_update_tick_error);
72
73 return mcl_success;
74 }
75
76 /**
77 * @brief Sets the initial value of the angle. Normally,
78 * the programme does the initial value setting automatically,
79 * but if you are not satisfied with the result, you can set the initial value.
80 * The initial value determines the zero point of the electrical angle.
81 * This means that the angle read by the sensor is calibrated to zero by the initial value
82 *
83 * @param encoder @ref mcl_encoder_t
84 * @param theta Used to calibrate electrical angle to zero
85 * @return hpm_mcl_stat_t @ref hpm_mcl_stat_t
86 */
hpm_mcl_encoder_set_initial_theta(mcl_encoder_t * encoder,float theta)87 hpm_mcl_stat_t hpm_mcl_encoder_set_initial_theta(mcl_encoder_t *encoder, float theta)
88 {
89 MCL_ASSERT(encoder != NULL, mcl_invalid_pointer);
90 MCL_ASSERT((encoder->status == encoder_status_run), mcl_encoder_not_ready);
91 encoder->theta_initial = theta;
92 return mcl_success;
93 }
94
hpm_mcl_encoder_process(mcl_encoder_t * encoder,uint32_t tick_deta)95 hpm_mcl_stat_t hpm_mcl_encoder_process(mcl_encoder_t *encoder, uint32_t tick_deta)
96 {
97 float ts;
98 float theta;
99 float speed;
100 float theta_forecast;
101 float theta_integrator;
102 float theta_deta;
103 hpm_mcl_stat_t status;
104
105 MCL_ASSERT_OPT(encoder != NULL, mcl_invalid_pointer);
106 MCL_ASSERT(tick_deta != 0, mcl_invalid_argument);
107 MCL_ASSERT((encoder->status == encoder_status_run), mcl_encoder_not_ready);
108 MCL_ASSERT_EXEC_CODE_AND_RETURN(encoder->cfg->callback.get_theta(&theta) == mcl_success,
109 encoder->status = encoder_status_fail, mcl_encoder_get_theta_error);
110 if (encoder->force_set_theta.enable) {
111 theta = encoder->force_set_theta.value;
112 } else {
113 theta = MCL_ANGLE_MOD_X(0, 2 * MCL_PI, theta - encoder->theta_initial);
114 }
115 switch (encoder->cfg->speed_cal_method) {
116 case encoder_method_user:
117 status = encoder->cfg->callback.process_by_user(theta, &speed, &theta_forecast);
118 MCL_ASSERT(status == mcl_success, status);
119 break;
120 case encoder_method_m:
121 ts = (float)tick_deta / (*encoder->mcu_clock_tick);
122 theta_deta = MCL_ANGLE_MOD_X((-MCL_PI), MCL_PI, (theta - encoder->cal_speed.m_method.theta_last));
123 if (MCL_FLOAT_IS_ZERO(theta_deta)) {
124 encoder->cal_speed.m_method.ts_sigma += ts;
125 } else {
126 encoder->cal_speed.m_method.ts_sigma = 0;
127 }
128 speed = theta_deta / ts;
129 if (encoder->cal_speed.m_method.ts_sigma >= encoder->cfg->timeout_s) {
130 speed = 0;
131 encoder->cal_speed.m_method.ts_sigma = encoder->cfg->timeout_s + 1;
132 }
133 encoder->cal_speed.m_method.theta_last = theta;
134 break;
135 case encoder_method_t:
136 ts = tick_deta / (*encoder->mcu_clock_tick);
137 theta_deta = MCL_ANGLE_MOD_X((-MCL_PI), MCL_PI, (theta - encoder->cal_speed.t_method.theta_last));
138 if (MCL_FLOAT_IS_ZERO(theta_deta)) {
139 encoder->cal_speed.t_method.ts_sigma += ts;
140 speed = encoder->cal_speed.t_method.speed_last;
141 } else {
142 speed = theta_deta / encoder->cal_speed.t_method.ts_sigma;
143 encoder->cal_speed.t_method.ts_sigma = 0;
144 }
145 if (encoder->cal_speed.t_method.ts_sigma >= encoder->cfg->timeout_s) {
146 speed = 0;
147 encoder->cal_speed.t_method.ts_sigma = encoder->cfg->timeout_s + 1;
148 }
149 encoder->cal_speed.t_method.theta_last = theta;
150 encoder->cal_speed.t_method.speed_last = speed;
151 break;
152 case encoder_method_m_t:
153 ts = tick_deta / (*encoder->mcu_clock_tick);
154 theta_deta = MCL_ANGLE_MOD_X((-MCL_PI), MCL_PI, (theta - encoder->cal_speed.m_t_method.theta_last));
155 encoder->cal_speed.m_t_method.ts_sigma += ts;
156 encoder->cal_speed.m_t_method.theta_sigma += theta_deta;
157 if (fabs(encoder->cal_speed.m_t_method.speed_filter_last) > encoder->cfg->speed_abs_switch_m_t) {
158 speed = encoder->cal_speed.m_t_method.theta_sigma / encoder->cal_speed.m_t_method.ts_sigma;
159 encoder->cal_speed.m_t_method.ts_sigma = 0;
160 encoder->cal_speed.m_t_method.theta_sigma = 0;
161 } else {
162 if (theta != encoder->cal_speed.m_t_method.theta_last) {
163 speed = encoder->cal_speed.m_t_method.theta_sigma / encoder->cal_speed.m_t_method.ts_sigma;
164 encoder->cal_speed.m_t_method.ts_sigma = 0;
165 encoder->cal_speed.m_t_method.theta_sigma = 0;
166 } else {
167 speed = encoder->cal_speed.m_t_method.speed_last;
168 }
169 }
170 encoder->cal_speed.m_t_method.speed_last = speed;
171 encoder->cal_speed.m_t_method.theta_last = theta;
172 break;
173 case encoder_method_pll:
174 theta_deta = sinf(theta) * cosf(encoder->cal_speed.pll_method.theta_last) - cosf(theta) * sinf(encoder->cal_speed.pll_method.theta_last);
175 encoder->cal_speed.pll_method.pi_integrator += theta_deta * encoder->cal_speed.pll_method.cfg->ki;
176 speed = theta_deta * encoder->cal_speed.pll_method.cfg->kp + encoder->cal_speed.pll_method.pi_integrator;
177 theta_integrator = speed * (*encoder->cal_speed.pll_method.period_call_time_s) + encoder->cal_speed.pll_method.theta_last;
178 theta_integrator = MCL_ANGLE_MOD_X(0, 2 * MCL_PI, theta_integrator);
179 encoder->cal_speed.pll_method.theta_last = theta_integrator;
180 break;
181 default:
182 return mcl_invalid_argument;
183 }
184 /**
185 * @brief iir filter
186 *
187 */
188 speed = hpm_mcl_filter_iir_df1(encoder->iirfilter, speed);
189 theta = MCL_ANGLE_MOD_X(0, 2 * MCL_PI, theta * (*encoder->pole_num));
190 theta_forecast = MCL_ANGLE_MOD_X(0, 2 * MCL_PI, theta + speed * (*encoder->current_loop_ts) * (*encoder->pole_num));
191
192 encoder->result.theta = theta;
193 encoder->result.speed = speed;
194 encoder->result.theta_forecast = theta_forecast;
195 encoder->cal_speed.m_t_method.speed_filter_last = encoder->result.speed;
196 return mcl_success;
197 }
198
hpm_mcl_encoder_set_status(mcl_encoder_t * encoder,mcl_encoder_status_t status)199 hpm_mcl_stat_t hpm_mcl_encoder_set_status(mcl_encoder_t *encoder, mcl_encoder_status_t status)
200 {
201 MCL_ASSERT(encoder != NULL, mcl_invalid_pointer);
202 encoder->status = status;
203 return mcl_success;
204 }
205
hpm_mcl_encoder_force_theta(mcl_encoder_t * encoder,float theta,bool enable)206 void hpm_mcl_encoder_force_theta(mcl_encoder_t *encoder, float theta, bool enable)
207 {
208 encoder->force_set_theta.enable = enable;
209 encoder->force_set_theta.value = theta;
210 }
211
hpm_mcl_encoder_get_raw_theta(mcl_encoder_t * encoder)212 float hpm_mcl_encoder_get_raw_theta(mcl_encoder_t *encoder)
213 {
214 float theta;
215
216 MCL_ASSERT(encoder->cfg->callback.get_theta(&theta) == mcl_success, -mcl_fail);
217
218 return theta;
219 }
220
hpm_mcl_encoder_get_theta(mcl_encoder_t * encoder)221 float hpm_mcl_encoder_get_theta(mcl_encoder_t *encoder)
222 {
223 return encoder->result.theta;
224 }
225
hpm_mcl_encoder_get_speed(mcl_encoder_t * encoder)226 float hpm_mcl_encoder_get_speed(mcl_encoder_t *encoder)
227 {
228 return encoder->result.speed;
229 }
230
hpm_mcl_encoder_get_forecast_theta(mcl_encoder_t * encoder)231 float hpm_mcl_encoder_get_forecast_theta(mcl_encoder_t *encoder)
232 {
233 return encoder->result.theta_forecast;
234 }
235
hpm_mcl_encoder_get_absolute_theta(mcl_encoder_t * encoder,float * theta)236 hpm_mcl_stat_t hpm_mcl_encoder_get_absolute_theta(mcl_encoder_t *encoder, float *theta)
237 {
238 MCL_ASSERT(encoder->cfg->callback.get_absolute_theta(theta) == mcl_success, mcl_encoder_get_theta_error);
239 return mcl_success;
240 }