sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

industry/foc foc_ident: adding flux linkage identification

Browse Source
This commit is contained in:
zouboan 2022-05-31 20:12:16 +08:00 committed by Xiang Xiao
parent 5ae5ab7f44
commit 30dbdf71ff
3 changed files with 248 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
include/industry/foc/float/foc_ident.h
View File

@ -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
};
/****************************************************************************

10
industry/foc/Kconfig
View File

@ -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

223
industry/foc/float/foc_ident.c
View File

@ -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;