You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2022/05/31 17:06:33 UTC
[incubator-nuttx-apps] 02/02: industry/foc foc_ident: adding flux linkage identification
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 <ff...@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;