From ede994d3fb1c464bd1f182bdebe09a98873bd9f9 Mon Sep 17 00:00:00 2001 From: Duke Date: Mon, 15 Jul 2024 14:44:29 +0800 Subject: [PATCH] rename current pid output identifier from 'i' to 'v' --- doc/block_diagram.svg | 6 +-- mdrv_fw/usr/app_main.h | 4 +- mdrv_fw/usr/app_motor.c | 93 +++++++++++++++++++++-------------------- mdrv_fw/usr/config.c | 8 ++-- 4 files changed, 56 insertions(+), 55 deletions(-) diff --git a/doc/block_diagram.svg b/doc/block_diagram.svg index 0127ece..e2920ee 100644 --- a/doc/block_diagram.svg +++ b/doc/block_diagram.svg @@ -947,7 +947,7 @@ style="font-size:3.7932px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:0.711226px" x="88.597168" y="238.60515" - id="tspan23-3-6">(cal_i_sq) + id="tspan23-3-6">(cal_v_sq) (cal_i_sd) + id="tspan23-3-6-0">(cal_v_sd) (i_alpha, i_beta) + id="tspan23-3-6-6">(v_alpha, v_beta) = (float)M_PI*2) + csa.cali_angle_elec -= (float)M_PI*2; + sin_tmp_angle_elec = sinf(csa.cali_angle_elec); + cos_tmp_angle_elec = cosf(csa.cali_angle_elec); + } else { + sin_tmp_angle_elec = sinf(csa.sen_angle_elec); + cos_tmp_angle_elec = cosf(csa.sen_angle_elec); + } if (dbg_str) d_debug_c(" a %d.%.2d %d.%.2d %d.%.2d", P_2F(angle_mech), P_2F(csa.sen_angle_elec), P_2F(csa.cali_angle_elec)); @@ -440,8 +448,8 @@ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) float i_alpha = ia; float i_beta = (ia + ib * 2) / 1.732050808f; // √3 - csa.sen_i_sq = -i_alpha * sin_sen_angle_elec + i_beta * cos_sen_angle_elec; - csa.sen_i_sd = i_alpha * cos_sen_angle_elec + i_beta * sin_sen_angle_elec; + csa.sen_i_sq = -i_alpha * sin_tmp_angle_elec + i_beta * cos_tmp_angle_elec; + csa.sen_i_sd = i_alpha * cos_tmp_angle_elec + i_beta * sin_tmp_angle_elec; if (dbg_str) //d_debug_c(", i %5d %5d %5d", ia, ib, ic); @@ -452,51 +460,44 @@ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) // current --> pwm if (csa.state != ST_STOP) { - float i_alpha, i_beta; - - // calculate angle - if (csa.state != ST_CALI) { + float v_alpha, v_beta; + // sign select direction for cali_current + int32_t target_current = (csa.state == ST_CALI) ? -csa.cali_current : csa.cal_current; #if defined(CAL_CURRENT_SMOOTH) - static bool near_limit = false; - if (!near_limit) { - if (fabsf(csa.pid_i_sq.target - csa.cal_current) <= 0.01f) - pid_f_set_target(&csa.pid_i_sq, csa.cal_current); - else - csa.pid_i_sq.target += sign(csa.cal_current - csa.pid_i_sq.target) * 0.01f; - } else { - csa.pid_i_sq.target -= sign(csa.pid_i_sq.target) * 0.005f; - } + static bool near_limit = false; + if (!near_limit) { + if (fabsf(csa.pid_i_sq.target - target_current) <= 0.01f) + pid_f_set_target(&csa.pid_i_sq, target_current); + else + csa.pid_i_sq.target += sign(target_current - csa.pid_i_sq.target) * 0.01f; + } else { + csa.pid_i_sq.target -= sign(csa.pid_i_sq.target) * 0.005f; + } #else - pid_f_set_target(&csa.pid_i_sq, csa.cal_current); + pid_f_set_target(&csa.pid_i_sq, target_current); #endif - csa.cal_i_sq = pid_f_compute_no_d(&csa.pid_i_sq, csa.sen_i_sq); - csa.cal_i_sd = pid_f_compute_no_d(&csa.pid_i_sd, csa.sen_i_sd); // target default 0 - // rotate 2d vector, origin: (sd, sq), after: (alpha, beta) - i_alpha = csa.cal_i_sd * cos_sen_angle_elec - csa.cal_i_sq * sin_sen_angle_elec; - i_beta = csa.cal_i_sd * sin_sen_angle_elec + csa.cal_i_sq * cos_sen_angle_elec; - // limit vector magnitude - float norm = sqrtf(i_alpha * i_alpha + i_beta * i_beta); - float limit = 2047 * 1.15f; // deliver 15% more power by svpwm - if (norm > limit) { - i_alpha *= limit / norm; - i_beta *= limit / norm; - vector_over_limit = norm; // for debug - } + csa.cal_v_sq = pid_f_compute_no_d(&csa.pid_i_sq, csa.sen_i_sq); + csa.cal_v_sd = pid_f_compute_no_d(&csa.pid_i_sd, csa.sen_i_sd); // target default 0 + if (csa.state == ST_CALI) + csa.cal_v_sd = 0; + + // rotate 2d vector, origin: (sd, sq), after: (alpha, beta) + v_alpha = csa.cal_v_sd * cos_tmp_angle_elec - csa.cal_v_sq * sin_tmp_angle_elec; + v_beta = csa.cal_v_sd * sin_tmp_angle_elec + csa.cal_v_sq * cos_tmp_angle_elec; + // limit vector magnitude + float norm = sqrtf(v_alpha * v_alpha + v_beta * v_beta); + float limit = 2047 * 1.15f; // deliver 15% more power by svpwm + if (norm > limit) { + v_alpha *= limit / norm; + v_beta *= limit / norm; + vector_over_limit = norm; // for debug + } #if defined(CAL_CURRENT_SMOOTH) - near_limit = norm > (limit * 0.97f); // 0.96 + near_limit = norm > (limit * 0.97f); // 0.96 #endif - } else { - csa.cali_angle_elec += csa.cali_angle_step; - if (csa.cali_angle_elec >= (float)M_PI*2) - csa.cali_angle_elec -= (float)M_PI*2; - - csa.cal_i_sq = -csa.cali_current; // sign select direction - i_alpha = -csa.cal_i_sq * sinf(csa.cali_angle_elec); - i_beta = csa.cal_i_sq * cosf(csa.cali_angle_elec); - } - out_pwm_u = lroundf(i_alpha); - out_pwm_v = lroundf(-i_alpha / 2 + i_beta * 0.866025404f); // (√3÷2) + out_pwm_u = lroundf(v_alpha); + out_pwm_v = lroundf(-v_alpha / 2 + v_beta * 0.866025404f); // (√3÷2) out_pwm_w = -out_pwm_u - out_pwm_v; // avoid over range again int16_t out_min = min(out_pwm_u, min(out_pwm_v, out_pwm_w)); @@ -512,9 +513,9 @@ void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) append_dprintf(DBG_CURRENT, "w:%d", out_pwm_w); */ if (dbg_str) - d_debug_c(", o %5d.%.2d %5d %5d %5d\n", P_2F(csa.cal_i_sq), out_pwm_u, out_pwm_v, out_pwm_w); + d_debug_c(", o %5d.%.2d %5d %5d %5d\n", P_2F(csa.cal_v_sq), out_pwm_u, out_pwm_v, out_pwm_w); } else { - csa.cal_i_sq = 0; + csa.cal_v_sq = 0; pid_f_set_target(&csa.pid_i_sq, 0); pid_f_set_target(&csa.pid_i_sd, 0); pid_f_reset(&csa.pid_i_sq, 0, 0); diff --git a/mdrv_fw/usr/config.c b/mdrv_fw/usr/config.c index ded875d..5a8b732 100644 --- a/mdrv_fw/usr/config.c +++ b/mdrv_fw/usr/config.c @@ -17,7 +17,7 @@ regr_t csa_w_allow[] = { { .offset = offsetof(csa_t, pid_i_sq), .size = offsetof(pid_f_t, target) }, { .offset = offsetof(csa_t, pid_i_sd), .size = offsetof(pid_f_t, target) }, { .offset = offsetof(csa_t, peak_cur_threshold), - .size = offsetof(csa_t, cal_i_sq) - offsetof(csa_t, peak_cur_threshold) } + .size = offsetof(csa_t, cal_v_sq) - offsetof(csa_t, peak_cur_threshold) } }; csa_hook_t csa_w_hook[] = { @@ -95,7 +95,7 @@ const csa_t csa_dft = { // i_term, last_input { .offset = offsetof(csa_t, pid_i_sd) + offsetof(pid_f_t, i_term), .size = 4 * 2 }, { .offset = offsetof(csa_t, sen_encoder), .size = 2 }, - { .offset = offsetof(csa_t, cal_i_sq), .size = 4 * 2 } + { .offset = offsetof(csa_t, cal_v_sq), .size = 4 * 2 } //{ .offset = offsetof(csa_t, noc_encoder), .size = 2 } }, { // speed { .offset = offsetof(csa_t, pid_speed) + offsetof(pid_f_t, target), .size = 4 * 3 }, @@ -330,8 +330,8 @@ void csa_list_show(void) CSA_SHOW(1, cal_pos, "pos loop target"); CSA_SHOW(1, cal_speed, "speed loop target"); CSA_SHOW(0, cal_current, "cur loop target"); - CSA_SHOW(0, cal_i_sq, "i_sq info"); - CSA_SHOW(0, cal_i_sd, "i_sd info"); + CSA_SHOW(0, cal_v_sq, "v_sq info"); + CSA_SHOW(0, cal_v_sd, "v_sd info"); d_info("\n"); debug_flush(true); CSA_SHOW(1, ori_encoder, "noc_encoder before add offset");