diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 7e604e399..2b193df9b 100644 --- a/examples/foc/Kconfig +++ b/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 diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index d3710ee04..155b44379 100644 --- a/examples/foc/foc_cfg.h +++ b/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) && \ diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index 44c831ffb..7be423ebd 100644 --- a/examples/foc/foc_motor_b16.c +++ b/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 */ diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index f368e2382..0c0e238ed 100644 --- a/examples/foc/foc_motor_b16.h +++ b/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 */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index aba1f76d5..f8e7730b6 100644 --- a/examples/foc/foc_motor_f32.c +++ b/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 */ diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 6bab5119a..dff9c8483 100644 --- a/examples/foc/foc_motor_f32.h +++ b/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 */ diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h index 95b41741e..d1efd90bc 100644 --- a/examples/foc/foc_thr.h +++ b/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,