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 2021/11/07 09:44:11 UTC
[incubator-nuttx-apps] 02/02: examples/foc: add Qenco support
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
commit 15b66aa1288051b4b5ba7796df67df44b851c736
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Nov 6 18:44:30 2021 +0100
examples/foc: add Qenco support
---
examples/foc/Kconfig | 29 ++++++++++++++++++++-
examples/foc/foc_cfg.h | 11 ++++++++
examples/foc/foc_motor_b16.c | 61 ++++++++++++++++++++++++++++++++++++++++++--
examples/foc/foc_motor_b16.h | 5 ++++
examples/foc/foc_motor_f32.c | 61 ++++++++++++++++++++++++++++++++++++++++++--
examples/foc/foc_motor_f32.h | 5 ++++
6 files changed, 167 insertions(+), 5 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index bd0062a..60a243a 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -90,10 +90,37 @@ endchoice #
if EXAMPLES_FOC_SENSORED
+choice
+ prompt "FOC sensored sensor selection"
+
+config EXAMPLES_FOC_HAVE_QENCO
+ bool "FOC example have qencoder"
+ select INDUSTRY_FOC_ANGLE_QENCO
+
config EXAMPLES_FOC_HAVE_HALL
bool "FOC example Hall sensor support"
select INDUSTRY_FOC_ANGLE_HALL
- default n
+
+endchoice # FOC sensored sensor selection
+
+if EXAMPLES_FOC_HAVE_QENCO
+
+config EXAMPLES_FOC_MOTOR_POLES
+ int "FOC example motor poles pairs"
+ default 0
+
+config EXAMPLES_FOC_QENCO_POSMAX
+ int "FOC example qencoder maximum position"
+ default 0
+
+config EXAMPLES_FOC_QENCO_DEVPATH
+ string "FOC example qencoder path prefix"
+ default "/dev/qe"
+ ---help---
+ The default path to the qenco device without the device minor number.
+ Default: /dev/qenco
+
+endif # EXAMPLES_FOC_HAVE_QENCO
if EXAMPLES_FOC_HAVE_HALL
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 8803499..47b3a5c 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -117,4 +117,15 @@
# endif
#endif
+/* Qenco configuration */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+# if CONFIG_EXAMPLES_FOC_MOTOR_POLES == 0
+# error
+# endif
+# if CONFIG_EXAMPLES_FOC_MOTOR_POSMAX == 0
+# error
+# endif
+#endif
+
#endif /* __EXAMPLES_FOC_FOC_CFG_H */
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index e43bca2..749564b 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg);
+#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
+ /* Configure motor poles */
+
+ motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Initialize PMSM model */
@@ -668,6 +674,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
struct foc_openloop_cfg_b16_s ol_cfg;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ struct foc_qenco_cfg_b16_s qe_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_b16_s hl_cfg;
#endif
@@ -704,6 +713,37 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ /* Initialize qenco angle handler */
+
+ ret = foc_angle_init_b16(&motor->qenco,
+ &g_foc_angle_qe_b16);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get qenco devpath */
+
+ sprintf(motor->qedpath,
+ "%s%d",
+ CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
+ motor->envp->id);
+
+ /* Configure qenco angle handler */
+
+ qe_cfg.posmax = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
+ qe_cfg.devpath = motor->qedpath;
+
+ ret = foc_angle_cfg_b16(&motor->qenco, &qe_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Initialize hall angle handler */
@@ -758,6 +798,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
/* Connect align callbacks private data */
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ align_cfg.cb.priv = &motor->qenco;
+# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
align_cfg.cb.priv = &motor->hall;
# endif
@@ -846,6 +889,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
motor->angle_ol = aout.angle;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ ret = foc_angle_run_b16(&motor->qenco, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
if (ret < 0)
@@ -867,9 +919,14 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
else if (aout.type == FOC_ANGLE_TYPE_MECH)
{
- /* TODO */
+ /* Store mechanical angle */
- ASSERT(0);
+ motor->angle_m = aout.angle;
+
+ /* Convert mechanical angle to electrical angle */
+
+ motor->angle_el = (b16muli(motor->angle_m,
+ motor->poles) % MOTOR_ANGLE_E_MAX_B16);
}
else
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 84d708a..86bc5e5 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -75,6 +75,10 @@ struct foc_motor_b16_s
char hldpath[32]; /* Hall devpath */
foc_angle_b16_t hall; /* Hall angle handler */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ char qedpath[32]; /* Qenco devpath */
+ foc_angle_b16_t qenco; /* Qenco angle handler */
+#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
b16_t vbus; /* Power bus voltage */
@@ -94,6 +98,7 @@ struct foc_motor_b16_s
b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */
b16_t pwm_duty_max; /* PWM duty max */
+ uint8_t poles; /* Motor poles */
dq_frame_b16_t dq_ref; /* DQ reference */
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
foc_handler_b16_t handler; /* FOC controller */
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index c8eb3f3..f6d02df 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
+#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
+ /* Configure motor poles */
+
+ motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Initialize PMSM model */
@@ -654,6 +660,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
struct foc_openloop_cfg_f32_s ol_cfg;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ struct foc_qenco_cfg_f32_s qe_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_f32_s hl_cfg;
#endif
@@ -690,6 +699,37 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ /* Initialize qenco angle handler */
+
+ ret = foc_angle_init_f32(&motor->qenco,
+ &g_foc_angle_qe_f32);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get qenco devpath */
+
+ sprintf(motor->qedpath,
+ "%s%d",
+ CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
+ motor->envp->id);
+
+ /* Configure qenco angle handler */
+
+ qe_cfg.posmax = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
+ qe_cfg.devpath = motor->qedpath;
+
+ ret = foc_angle_cfg_f32(&motor->qenco, &qe_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Initialize hall angle handler */
@@ -744,6 +784,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
/* Connect align callbacks private data */
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ align_cfg.cb.priv = &motor->qenco;
+# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
align_cfg.cb.priv = &motor->hall;
# endif
@@ -843,6 +886,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
motor->angle_ol = aout.angle;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ ret = foc_angle_run_f32(&motor->qenco, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
if (ret < 0)
@@ -864,9 +916,14 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
else if (aout.type == FOC_ANGLE_TYPE_MECH)
{
- /* TODO */
+ /* Store mechanical angle */
- ASSERT(0);
+ motor->angle_m = aout.angle;
+
+ /* Convert mechanical angle to electrical angle */
+
+ motor->angle_el = fmodf(motor->angle_m * motor->poles,
+ MOTOR_ANGLE_E_MAX);
}
else
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index f69e41a..d07f361 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -75,6 +75,10 @@ struct foc_motor_f32_s
char hldpath[32]; /* Hall devpath */
foc_angle_f32_t hall; /* Hall angle handler */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+ char qedpath[32]; /* Qenco devpath */
+ foc_angle_f32_t qenco; /* Qenco angle handler */
+#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
float vbus; /* Power bus voltage */
@@ -94,6 +98,7 @@ struct foc_motor_f32_s
float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */
float pwm_duty_max; /* PWM duty max */
+ uint8_t poles; /* Motor poles */
dq_frame_f32_t dq_ref; /* DQ reference */
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
foc_handler_f32_t handler; /* FOC controller */