This is an automated email from the ASF dual-hosted git repository. xiaoxiang pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git
commit 30dbdf71ff99e21f1aba746b408df74cddf1e32b Author: zouboan <f...@feedforward.com.cn> AuthorDate: Tue May 31 20:12:16 2022 +0800 industry/foc foc_ident: adding flux linkage identification --- include/industry/foc/float/foc_ident.h | 39 +++++- industry/foc/Kconfig | 10 ++ industry/foc/float/foc_ident.c | 223 +++++++++++++++++++++++++++++---- 3 files changed, 248 insertions(+), 24 deletions(-) diff --git a/include/industry/foc/float/foc_ident.h b/include/industry/foc/float/foc_ident.h index d7172f89a..c8e3fbaa9 100644 --- a/include/industry/foc/float/foc_ident.h +++ b/include/industry/foc/float/foc_ident.h @@ -35,13 +35,47 @@ * Public Type Definition ****************************************************************************/ +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX +/* Identification routine callbacks */ + +struct foc_routine_ident_cb_f32_s +{ + /* Private data for angle callbacks */ + + FAR void *priv_angle; + + /* Private data for speed callbacks */ + + FAR void *priv_speed; + + /* Openloop angle zero callback */ + + CODE int (*zero)(FAR void *priv); + + /* Identification openloop angle callback */ + + CODE float (*angle)(FAR void *priv, float speed, float dir); + + /* Identification openloop speed callback */ + + CODE float (*speed)(FAR void *priv, float des, float now); +}; +#endif + /* Identification routine configuration */ struct foc_routine_ident_cfg_f32_s { +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + struct foc_routine_ident_cb_f32_s cb; /* Identification routine callbacks */ + + float flux_vel; /* Flux linkage measurement velocity */ + float flux_volt; /* Flux linkage measurement voltage */ + int flux_steps; /* Flux linkage measurement steps */ +#endif float per; /* Routine period in sec */ float res_current; /* Resistance measurement current */ - float ind_volt; /* Inductance measurement current */ + float ind_volt; /* Inductance measurement voltage */ int res_steps; /* Resistance measurement steps */ int ind_steps; /* Inductance measurement steps */ int idle_steps; /* IDLE steps */ @@ -54,6 +88,9 @@ struct foc_routine_ident_final_f32_s bool ready; /* Result ready */ float res; /* Phase resistance */ float ind; /* Phase inductance */ +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + float flux; /* Motor flux linkage */ +#endif }; /**************************************************************************** diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig index a6e55c169..15719f6c9 100644 --- a/industry/foc/Kconfig +++ b/industry/foc/Kconfig @@ -149,6 +149,16 @@ config INDUSTRY_FOC_IDENT ---help--- Enable support for motor identification routine (phase resistance and phase inductance) +if INDUSTRY_FOC_IDENT + +config INDUSTRY_FOC_IDENT_FLUX + bool "FOC flux linkage identification support" + default n + ---help--- + Enable support for flux linkage identification + +endif # INDUSTRY_FOC_IDENT + config INDUSTRY_FOC_VELOCITY_ODIV bool "FOC velocity DIV observer" default n diff --git a/industry/foc/float/foc_ident.c b/industry/foc/float/foc_ident.c index 99e824acd..528abecab 100644 --- a/industry/foc/float/foc_ident.c +++ b/industry/foc/float/foc_ident.c @@ -55,6 +55,10 @@ enum foc_ident_run_stage_e FOC_IDENT_RUN_IDLE2, FOC_IDENT_RUN_IND, FOC_IDENT_RUN_IDLE3, +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + FOC_IDENT_RUN_FLU, + FOC_IDENT_RUN_IDLE4, +#endif FOC_IDENT_RUN_DONE }; @@ -67,6 +71,27 @@ struct foc_ident_f32_s pid_controller_f32_t pi; /* PI controller for res */ int cntr; /* Helper counter */ int stage; /* Ident stage */ + + /* global data in resistance identification */ + + float curr_sum; + float volt_sum; + + /* global data in inductance identification */ + + float sign; + float curr1_sum; + float curr2_sum; +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + /* global data in flux linkage identification */ + + float vq_sum; + float vd_sum; + float iq_sum; + float id_sum; + float vel; + int aveg_sum; +#endif }; /**************************************************************************** @@ -164,8 +189,6 @@ int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident, int ret = FOC_ROUTINE_RUN_NOTDONE; float err = 0.0f; float vref = 0.0f; - static float curr_sum = 0.0f; - static float volt_sum = 0.0f; /* Initialize PI controller */ @@ -197,15 +220,17 @@ int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident, ident->cntr += 1; if (ident->cntr > (ident->cfg.res_steps / 3)) { - volt_sum += vector2d_mag(in->foc_state->vdq.q, in->foc_state->vdq.d); - curr_sum += vector2d_mag(in->foc_state->idq.q, in->foc_state->idq.d); + ident->volt_sum += vector2d_mag(in->foc_state->vdq.q, + in->foc_state->vdq.d); + ident->curr_sum += vector2d_mag(in->foc_state->idq.q, + in->foc_state->idq.d); } if (ident->cntr > ident->cfg.res_steps) { /* Get resistance */ - ident->final.res = (2.0f / 3.0f) * volt_sum / curr_sum; + ident->final.res = (2.0f / 3.0f) * ident->volt_sum / ident->curr_sum; /* Force IDLE state */ @@ -224,10 +249,10 @@ int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident, ident->cntr = 0; - /* Reset static data */ + /* Reset static curr_sum and volt_sum */ - curr_sum = 0.0f; - volt_sum = 0.0f; + ident->curr_sum = 0.0f; + ident->volt_sum = 0.0f; } return ret; @@ -255,31 +280,28 @@ int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident, float curr1_avg = 0.0f; float curr2_avg = 0.0f; float delta_curr = 0.0f; - static float sign = 1.0f; - static float curr1_sum = 0.0f; - static float curr2_sum = 0.0f; /* If previous sign was -1 then we have top current, * if previous sing was +1 then we have bottom current. */ - if (sign > 0) + if (ident->sign > 0) { /* Average bottm current */ - curr1_sum += in->foc_state->idq.d; + ident->curr1_sum += in->foc_state->idq.d; } else { /* Average top current */ - curr2_sum += in->foc_state->idq.d; + ident->curr2_sum += in->foc_state->idq.d; } /* Invert voltage to generate square wave D voltage */ - sign = -sign; - vref = sign * ident->cfg.ind_volt; + ident->sign = -ident->sign; + vref = ident->sign * ident->cfg.ind_volt; /* Force alpha voltage = vref */ @@ -298,8 +320,8 @@ int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident, { /* Half samples from curr1, other half from curr2 */ - curr1_avg = 2 * curr1_sum / ident->cntr; - curr2_avg = 2 * curr2_sum / ident->cntr; + curr1_avg = 2 * ident->curr1_sum / ident->cntr; + curr2_avg = 2 * ident->curr2_sum / ident->cntr; /* Average delta current */ @@ -333,16 +355,134 @@ int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident, ident->cntr = 0; - /* Reset static data */ + /* Reset curr1_sum curr2_sum and sign */ - sign = 1.0f; - curr1_sum = 0.0f; - curr2_sum = 0.0f; + ident->sign = 1.0f; + ident->curr1_sum = 0.0f; + ident->curr2_sum = 0.0f; } return ret; } +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX +/**************************************************************************** + * Name: foc_ident_fluxlink_run_f32 + * + * Description: + * Run flux linkage identification routine + * + * Input Parameter: + * ident - pointer to FOC ident routine + * in - pointer to FOC routine input data + * out - pointer to FOC routine output data + * + ****************************************************************************/ + +int foc_ident_fluxlink_run_f32(FAR struct foc_ident_f32_s *ident, + FAR struct foc_routine_in_f32_s *in, + FAR struct foc_routine_out_f32_s *out) +{ + int ret = FOC_ROUTINE_RUN_NOTDONE; + float curr_avg = 0.0f; + float volt_avg = 0.0f; + + DEBUGASSERT(ident->cfg.cb.zero != NULL); + DEBUGASSERT(ident->cfg.cb.angle != NULL); + DEBUGASSERT(ident->cfg.cb.speed != NULL); + + if (ident->cfg.cb.angle != NULL) + { + /* Update openloop angle by angle callbacks */ + + out->angle = ident->cfg.cb.angle(ident->cfg.cb.priv_angle, + ident->vel, DIR_CW); + } + + if (ident->cfg.cb.speed != NULL) + { + /* Update openloop speed by speed callbacks */ + + ident->vel = ident->cfg.cb.speed(ident->cfg.cb.priv_speed, + ident->cfg.flux_vel, ident->vel); + } + + /* Force q axis voltage = ident->cfg.flux_volt */ + + out->dq_ref.q = ident->cfg.flux_volt; + out->dq_ref.d = 0.0f; + out->vdq_comp.q = 0.0f; + out->vdq_comp.d = 0.0f; + out->foc_mode = FOC_HANDLER_MODE_VOLTAGE; + + /* Increase counter */ + + ident->cntr += 1; + if (ident->vel >= ident->cfg.flux_vel) + { + ident->vq_sum += in->foc_state->vdq.q; + ident->vd_sum += in->foc_state->vdq.d; + ident->iq_sum += in->foc_state->idq.q; + ident->id_sum += in->foc_state->idq.d; + ident->aveg_sum++; + } + + if (ident->cntr > ident->cfg.flux_steps) + { + volt_avg = vector2d_mag(ident->vq_sum / ident->aveg_sum, + ident->vd_sum / ident->aveg_sum); + curr_avg = vector2d_mag(ident->iq_sum / ident->aveg_sum, + ident->id_sum / ident->aveg_sum); + + /* Get flux linkage */ + + ident->final.flux = (volt_avg - ident->final.res * curr_avg) / + ident->vel; + + /* Force IDLE state */ + + out->dq_ref.q = 0.0f; + out->dq_ref.d = 0.0f; + out->vdq_comp.q = 0.0f; + out->vdq_comp.d = 0.0f; + out->angle = 0.0f; + out->foc_mode = FOC_HANDLER_MODE_IDLE; + + /* Reset global value of open loop angle */ + + if (ident->cfg.cb.zero != NULL) + { + ret = ident->cfg.cb.zero(ident->cfg.cb.priv_angle); + if (ret < 0) + { + FOCLIBERR("ERROR: ident zero callback failed %d!\n", ret); + goto errout; + } + } + + /* Flux linkage identification done */ + + ret = FOC_ROUTINE_RUN_DONE; + + /* Reset counter */ + + ident->cntr = 0; + + /* Reset global data */ + + ident->vq_sum = 0.0f; + ident->vd_sum = 0.0f; + ident->iq_sum = 0.0f; + ident->id_sum = 0.0f; + ident->vel = 0.0f; + ident->aveg_sum = 0; + } + +errout: + return ret; +} +#endif + /**************************************************************************** * Name: foc_routine_ident_init_f32 * @@ -356,6 +496,7 @@ int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident, int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r) { + FAR struct foc_ident_f32_s *i = NULL; int ret = OK; DEBUGASSERT(r); @@ -369,6 +510,9 @@ int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r) goto errout; } + i = r->data; + i->sign = 1.0f; + errout: return ret; } @@ -432,13 +576,20 @@ int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg) goto errout; } - if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f) + if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + || i->cfg.flux_volt <= 0.0f +#endif + ) { ret = -EINVAL; goto errout; } if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 || +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + i->cfg.flux_steps <= 0 || +#endif i->cfg.idle_steps <= 0) { ret = -EINVAL; @@ -502,6 +653,9 @@ int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r, case FOC_IDENT_RUN_IDLE1: case FOC_IDENT_RUN_IDLE2: case FOC_IDENT_RUN_IDLE3: +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + case FOC_IDENT_RUN_IDLE4: +#endif { /* De-energetize motor */ @@ -562,6 +716,29 @@ int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r, break; } +#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX + case FOC_IDENT_RUN_FLU: + { + /* Flux linkage */ + + ret = foc_ident_fluxlink_run_f32(i, in, out); + if (ret < 0) + { + goto errout; + } + + if (ret == FOC_ROUTINE_RUN_DONE) + { + FOCLIBLOG("IDENT FLU done!\n"); + + i->stage += 1; + ret = FOC_ROUTINE_RUN_NOTDONE; + } + + break; + } + +#endif case FOC_IDENT_RUN_DONE: { ret = FOC_ROUTINE_RUN_DONE;