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/08/27 17:20:59 UTC
[incubator-nuttx-apps] branch master updated: examples/foc: support for motor 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
The following commit(s) were added to refs/heads/master by this push:
new 7dbd02947 examples/foc: support for motor identification
7dbd02947 is described below
commit 7dbd02947e4d6699c15cb5bf8941cda173129193
Author: raiden00pl <ra...@railab.me>
AuthorDate: Mon Aug 15 14:36:04 2022 +0200
examples/foc: support for motor identification
---
examples/foc/Kconfig | 29 +++++++
examples/foc/foc_cfg.h | 17 ++++
examples/foc/foc_motor_b16.c | 183 +++++++++++++++++++++++++++++++++++++++++--
examples/foc/foc_motor_b16.h | 9 +++
examples/foc/foc_motor_f32.c | 170 ++++++++++++++++++++++++++++++++++++++--
examples/foc/foc_motor_f32.h | 9 +++
examples/foc/foc_thr.h | 3 +
7 files changed, 408 insertions(+), 12 deletions(-)
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,