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

industry/foc/fixed16/foc_ident.c: port changes from float32 implementation

Browse Source
This commit is contained in:
raiden00pl 2022-08-16 09:54:19 +02:00 committed by Xiang Xiao
parent 13bfad1053
commit 9517b46958
2 changed files with 44 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
industry/foc/fixed16/foc_ident.c
View File

@ -66,6 +66,17 @@ struct foc_ident_b16_s
pid_controller_b16_t pi; /* PI controller for res */ pid_controller_b16_t pi; /* PI controller for res */
int cntr; /* Helper counter */ int cntr; /* Helper counter */
int stage; /* Ident stage */ int stage; /* Ident stage */
/* global data in resistance identification */
b16_t curr_sum;
b16_t volt_sum;
/* global data in inductance identification */
b16_t sign;
b16_t curr1_sum;
b16_t curr2_sum;
}; };
/**************************************************************************** /****************************************************************************
@ -193,12 +204,21 @@ int foc_ident_res_run_b16(FAR struct foc_ident_b16_s *ident,
/* Increase counter */ /* Increase counter */
ident->cntr += 1; ident->cntr += 1;
if (ident->cntr > (ident->cfg.res_steps / 3))
{
ident->volt_sum += vector2d_mag_b16(in->foc_state->vdq.q,
in->foc_state->vdq.d);
ident->curr_sum += vector2d_mag_b16(in->foc_state->idq.q,
in->foc_state->idq.d);
}
if (ident->cntr > ident->cfg.res_steps) if (ident->cntr > ident->cfg.res_steps)
{ {
/* Get resistance */ /* Get resistance */
ident->final.res = b16divb16(vref, ident->cfg.res_current); ident->final.res = b16divb16(b16mulb16(
ftob16(2.0f / 3.0f),
ident->volt_sum),
ident->curr_sum);
/* Force IDLE state */ /* Force IDLE state */
@ -216,6 +236,11 @@ int foc_ident_res_run_b16(FAR struct foc_ident_b16_s *ident,
/* Reset counter */ /* Reset counter */
ident->cntr = 0; ident->cntr = 0;
/* Reset static curr_sum and volt_sum */
ident->curr_sum = 0;
ident->volt_sum = 0;
} }
return ret; return ret;
@ -243,9 +268,6 @@ int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
b16_t curr1_avg = 0; b16_t curr1_avg = 0;
b16_t curr2_avg = 0; b16_t curr2_avg = 0;
b16_t delta_curr = 0; b16_t delta_curr = 0;
static b16_t sign = b16ONE;
static b16_t curr1_sum = 0;
static b16_t curr2_sum = 0;
b16_t tmp1 = 0; b16_t tmp1 = 0;
b16_t tmp2 = 0; b16_t tmp2 = 0;
b16_t tmp3 = 0; b16_t tmp3 = 0;
@ -254,23 +276,23 @@ int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
* if previous sing was +1 then we have bottom current. * if previous sing was +1 then we have bottom current.
*/ */
if (sign > 0) if (ident->sign > 0)
{ {
/* Average bottm current */ /* Average bottm current */
curr1_sum += in->foc_state->idq.d; ident->curr1_sum += in->foc_state->idq.d;
} }
else else
{ {
/* Average top current */ /* 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 */ /* Invert voltage to generate square wave D voltage */
sign = -sign; ident->sign = -ident->sign;
vref = b16mulb16(sign, ident->cfg.ind_volt); vref = b16mulb16(ident->sign, ident->cfg.ind_volt);
/* Force alpha voltage = vref */ /* Force alpha voltage = vref */
@ -289,8 +311,8 @@ int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
{ {
/* Half samples from curr1, other half from curr2 */ /* Half samples from curr1, other half from curr2 */
tmp1 = b16muli(curr1_sum, 2); tmp1 = b16muli(ident->curr1_sum, 2);
tmp2 = b16muli(curr2_sum, 2); tmp2 = b16muli(ident->curr2_sum, 2);
curr1_avg = b16divb16(tmp1, ident->cntr); curr1_avg = b16divb16(tmp1, ident->cntr);
curr2_avg = b16divb16(tmp2, ident->cntr); curr2_avg = b16divb16(tmp2, ident->cntr);
@ -330,9 +352,9 @@ int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
/* Reset static data */ /* Reset static data */
sign = b16ONE; ident->sign = b16ONE;
curr1_sum = 0; ident->curr1_sum = 0;
curr2_sum = 0; ident->curr2_sum = 0;
} }
return ret; return ret;
@ -351,7 +373,8 @@ int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r) int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r)
{ {
int ret = OK; FAR struct foc_ident_b16_s *i = NULL;
int ret = OK;
DEBUGASSERT(r); DEBUGASSERT(r);
@ -364,6 +387,9 @@ int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r)
goto errout; goto errout;
} }
i = r->data;
i->sign = b16ONE;
errout: errout:
return ret; return ret;
} }
@ -566,6 +592,7 @@ int foc_routine_ident_run_b16(FAR foc_routine_b16_t *r,
case FOC_IDENT_RUN_DONE: case FOC_IDENT_RUN_DONE:
{ {
ret = FOC_ROUTINE_RUN_DONE; ret = FOC_ROUTINE_RUN_DONE;
i->stage = FOC_IDENT_RUN_INIT;
break; break;
} }

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

@ -81,6 +81,7 @@ struct foc_ident_f32_s
float sign; float sign;
float curr1_sum; float curr1_sum;
float curr2_sum; float curr2_sum;
#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX #ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
/* global data in flux linkage identification */ /* global data in flux linkage identification */