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

examples/foc: support for motor identification

Browse Source
This commit is contained in:
raiden00pl 2022-08-15 14:36:04 +02:00 committed by Xiang Xiao
parent f987c6c02e
commit 7dbd02947e
7 changed files with 408 additions and 12 deletions

29
examples/foc/Kconfig

@ -375,6 +375,35 @@ config EXAMPLES_FOC_ALIGN_SEC
endif # EXAMPLES_FOC_HAVE_ALIGN
config EXAMPLES_FOC_HAVE_IDENT
bool "FOC example motor identification support"
select INDUSTRY_FOC_IDENT
default n
if EXAMPLES_FOC_HAVE_IDENT
config EXAMPLES_FOC_IDENT_RES_CURRENT
int "FOC motor ident resistance current (x1000)"
default 0
config EXAMPLES_FOC_IDENT_IND_VOLTAGE
int "FOC motor ident inductance voltage (x1000)"
default 0
config EXAMPLES_FOC_IDENT_RES_SEC
int "FOC motor ident resistance time in sec (x1000)"
default 0
config EXAMPLES_FOC_IDENT_IND_SEC
int "FOC motor ident inductance time in sec (x1000)"
default 0
config EXAMPLES_FOC_IDENT_IDLE
int "FOC motor ident idle steps"
default 100
endif # EXAMPLES_FOC_HAVE_IDENT
endmenu # FOC controller parameters
config EXAMPLES_FOC_HAVE_RUN

17
examples/foc/foc_cfg.h

@ -82,6 +82,23 @@
# error
#endif
/* Motor identification support */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
# if (CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT == 0)
# error
# endif
# if (CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE == 0)
# error
# endif
# if (CONFIG_EXAMPLES_FOC_IDENT_RES_SEC == 0)
# error
# endif
# if (CONFIG_EXAMPLES_FOC_IDENT_IND_SEC == 0)
# error
# endif
#endif
/* Printer prescaler */
#if defined(CONFIG_INDUSTRY_FOC_HANDLER_PRINT) && \

183
examples/foc/foc_motor_b16.c

@ -35,6 +35,12 @@
* Pre-processor Definitions
****************************************************************************/
#define FOC_FLOAT_IDENT_RES_MIN ftob16(1e-6)
#define FOC_FLOAT_IDENT_RES_MAX ftob16(2.0f)
#define FOC_FLOAT_IDENT_IND_MIN ftob16(1e-9)
#define FOC_FLOAT_IDENT_IND_MAX ftob16(2.0f)
/****************************************************************************
* Private Type Definition
****************************************************************************/
@ -132,6 +138,91 @@ errout:
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/****************************************************************************
* Name: foc_motor_ident
****************************************************************************/
static int foc_motor_ident(FAR struct foc_motor_b16_s *motor, FAR bool *done)
{
struct foc_routine_in_b16_s in;
struct foc_routine_out_b16_s out;
struct foc_routine_ident_final_b16_s final;
int ret = OK;
/* Get input */
in.foc_state = &motor->foc_state;
in.angle = motor->angle_now;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
in.vel = motor->vel.now;
#endif
in.vbus = motor->vbus;
/* Run ident procedure */
ret = foc_routine_run_b16(&motor->ident, &in, &out);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_run_b16 failed %d!\n", ret);
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
ret = foc_routine_final_b16(&motor->ident, &final);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_final_b16 failed %d!\n", ret);
goto errout;
}
PRINTF("Ident results:\n");
PRINTF(" res = %.4f\n", b16tof(final.res));
PRINTF(" ind = %.8f\n", b16tof(final.ind));
if (final.res < FOC_FLOAT_IDENT_RES_MIN ||
final.res > FOC_FLOAT_IDENT_RES_MAX)
{
PRINTF("ERROR: Motor resistance out of valid range res=%.4f!\n",
b16tof(final.res));
ret = -EINVAL;
goto errout;
}
if (final.ind < FOC_FLOAT_IDENT_IND_MIN ||
final.ind > FOC_FLOAT_IDENT_IND_MAX)
{
PRINTF("ERROR: Motor inductance out of valid range ind=%.8f!\n",
b16tof(final.ind));
ret = -EINVAL;
goto errout;
}
/* Store results */
motor->phy_ident.res = final.res;
motor->phy_ident.ind = final.ind;
*done = true;
}
/* Copy output */
motor->dq_ref.d = out.dq_ref.d;
motor->dq_ref.q = out.dq_ref.q;
motor->vdq_comp.d = out.vdq_comp.d;
motor->vdq_comp.q = out.vdq_comp.q;
motor->angle_now = out.angle;
motor->foc_mode = out.foc_mode;
errout:
return ret;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/****************************************************************************
* Name: foc_runmode_init
@ -721,6 +812,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_b16_s align_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
struct foc_routine_ident_cfg_b16_s ident_cfg;
#endif
int ret = OK;
@ -867,11 +961,45 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Initialize motor identifiaction routine */
ret = foc_routine_init_b16(&motor->ident, &g_foc_routine_ident_b16);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_init_b16 failed %d!\n", ret);
goto errout;
}
/* Initialize motor identification data */
ident_cfg.per = motor->per;
ident_cfg.res_current = ftob16(CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT /
1000.0f);
ident_cfg.ind_volt = ftob16(CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE /
1000.0f);
ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ *
CONFIG_EXAMPLES_FOC_IDENT_RES_SEC / 1000);
ident_cfg.ind_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ *
CONFIG_EXAMPLES_FOC_IDENT_IND_SEC / 1000);
ident_cfg.idle_steps = CONFIG_EXAMPLES_FOC_IDENT_IDLE;
ret = foc_routine_cfg_b16(&motor->ident, &ident_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_ident_cfg_b16 failed %d!\n", ret);
goto errout;
}
#endif
/* Initialize controller state */
motor->ctrl_state = FOC_CTRL_STATE_INIT;
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_HAVE_RUN)
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_RUN) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_IDENT)
errout:
#endif
return ret;
@ -887,6 +1015,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Deinitialize motor alignment routine */
ret = foc_routine_deinit_b16(&motor->align);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_deinit_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Deinitialize motor identment routine */
ret = foc_routine_deinit_b16(&motor->ident);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_deinit_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Deinitialize PMSM model */
@ -1030,9 +1180,6 @@ errout:
int foc_motor_control(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
bool align_done = false;
#endif
DEBUGASSERT(motor);
@ -1055,14 +1202,38 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor)
{
/* Run motor align procedure */
ret = foc_motor_align(motor, &align_done);
ret = foc_motor_align(motor, &motor->align_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_align failed %d!\n", ret);
goto errout;
}
if (align_done == true)
if (motor->align_done == true)
{
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
case FOC_CTRL_STATE_IDENT:
{
/* Run motor identification procedure */
ret = foc_motor_ident(motor, &motor->ident_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_ident failed %d!\n", ret);
goto errout;
}
if (motor->ident_done == true)
{
/* Next state */

9
examples/foc/foc_motor_b16.h

@ -38,6 +38,9 @@
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
# include "industry/foc/fixed16/foc_align.h"
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
# include "industry/foc/fixed16/foc_ident.h"
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
# include "industry/foc/fixed16/foc_model.h"
#endif
@ -110,6 +113,12 @@ struct foc_motor_b16_s
struct foc_ramp_b16_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_b16_s align; /* Alignment routine */
bool align_done; /* Alignment done */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
struct foc_routine_b16_s ident; /* Motor ident routine */
struct motor_phy_params_b16_s phy_ident; /* Motor phy from ident */
bool ident_done; /* Motor ident done */
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_b16_s model; /* Model handler */

170
examples/foc/foc_motor_f32.c

@ -35,6 +35,12 @@
* Pre-processor Definitions
****************************************************************************/
#define FOC_FLOAT_IDENT_RES_MIN (1e-6)
#define FOC_FLOAT_IDENT_RES_MAX (2.0f)
#define FOC_FLOAT_IDENT_IND_MIN (1e-9)
#define FOC_FLOAT_IDENT_IND_MAX (2.0f)
/****************************************************************************
* Private Type Definition
****************************************************************************/
@ -132,6 +138,91 @@ errout:
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/****************************************************************************
* Name: foc_motor_ident
****************************************************************************/
static int foc_motor_ident(FAR struct foc_motor_f32_s *motor, FAR bool *done)
{
struct foc_routine_in_f32_s in;
struct foc_routine_out_f32_s out;
struct foc_routine_ident_final_f32_s final;
int ret = OK;
/* Get input */
in.foc_state = &motor->foc_state;
in.angle = motor->angle_now;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
in.vel = motor->vel.now;
#endif
in.vbus = motor->vbus;
/* Run ident procedure */
ret = foc_routine_run_f32(&motor->ident, &in, &out);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_run_f32 failed %d!\n", ret);
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
ret = foc_routine_final_f32(&motor->ident, &final);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_final_f32 failed %d!\n", ret);
goto errout;
}
PRINTF("Ident results:\n");
PRINTF(" res = %.4f\n", final.res);
PRINTF(" ind = %.8f\n", final.ind);
if (final.res < FOC_FLOAT_IDENT_RES_MIN ||
final.res > FOC_FLOAT_IDENT_RES_MAX)
{
PRINTF("ERROR: Motor resistance out of valid range res=%.4f!\n",
final.res);
ret = -EINVAL;
goto errout;
}
if (final.ind < FOC_FLOAT_IDENT_IND_MIN ||
final.ind > FOC_FLOAT_IDENT_IND_MAX)
{
PRINTF("ERROR: Motor inductance out of valid range ind=%.8f!\n",
final.ind);
ret = -EINVAL;
goto errout;
}
/* Store results */
motor->phy_ident.res = final.res;
motor->phy_ident.ind = final.ind;
*done = true;
}
/* Copy output */
motor->dq_ref.d = out.dq_ref.d;
motor->dq_ref.q = out.dq_ref.q;
motor->vdq_comp.d = out.vdq_comp.d;
motor->vdq_comp.q = out.vdq_comp.q;
motor->angle_now = out.angle;
motor->foc_mode = out.foc_mode;
errout:
return ret;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/****************************************************************************
* Name: foc_runmode_init
@ -705,6 +796,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_f32_s align_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
struct foc_routine_ident_cfg_f32_s ident_cfg;
#endif
int ret = OK;
@ -851,11 +945,43 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Initialize motor identifiaction routine */
ret = foc_routine_init_f32(&motor->ident, &g_foc_routine_ident_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_init_f32 failed %d!\n", ret);
goto errout;
}
/* Initialize motor identification data */
ident_cfg.per = motor->per;
ident_cfg.res_current = (CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT / 1000.0f);
ident_cfg.ind_volt = (CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE / 1000.0f);
ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \
CONFIG_EXAMPLES_FOC_IDENT_RES_SEC / 1000);
ident_cfg.ind_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \
CONFIG_EXAMPLES_FOC_IDENT_IND_SEC / 1000);
ident_cfg.idle_steps = CONFIG_EXAMPLES_FOC_IDENT_IDLE;
ret = foc_routine_cfg_f32(&motor->ident, &ident_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_ident_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
/* Initialize controller state */
motor->ctrl_state = FOC_CTRL_STATE_INIT;
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_HAVE_RUN)
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_RUN) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_IDENT)
errout:
#endif
return ret;
@ -882,6 +1008,17 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Deinitialize motor identment routine */
ret = foc_routine_deinit_f32(&motor->ident);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Deinitialize PMSM model */
@ -1025,9 +1162,6 @@ errout:
int foc_motor_control(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
bool align_done = false;
#endif
DEBUGASSERT(motor);
@ -1050,14 +1184,38 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor)
{
/* Run motor align procedure */
ret = foc_motor_align(motor, &align_done);
ret = foc_motor_align(motor, &motor->align_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_align failed %d!\n", ret);
goto errout;
}
if (align_done == true)
if (motor->align_done == true)
{
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
case FOC_CTRL_STATE_IDENT:
{
/* Run motor identification procedure */
ret = foc_motor_ident(motor, &motor->ident_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_ident failed %d!\n", ret);
goto errout;
}
if (motor->ident_done == true)
{
/* Next state */

9
examples/foc/foc_motor_f32.h

@ -38,6 +38,9 @@
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
# include "industry/foc/float/foc_align.h"
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
# include "industry/foc/float/foc_ident.h"
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
# include "industry/foc/float/foc_model.h"
#endif
@ -110,6 +113,12 @@ struct foc_motor_f32_s
struct foc_ramp_f32_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_f32_s align; /* Alignment routine */
bool align_done; /* Motor alignment done */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
struct foc_routine_f32_s ident; /* Motor ident routine */
struct motor_phy_params_f32_s phy_ident; /* Motor phy from ident */
bool ident_done; /* Motor ident done */
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_f32_s model; /* Model handler */

3
examples/foc/foc_thr.h

@ -84,6 +84,9 @@ enum foc_controller_state_e
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
FOC_CTRL_STATE_ALIGN,
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
FOC_CTRL_STATE_IDENT,
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
FOC_CTRL_STATE_RUN_INIT,
FOC_CTRL_STATE_RUN,