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:09 UTC
[incubator-nuttx-apps] branch master updated (f38d7d2 -> 15b66aa)
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git.
from f38d7d2 examples/foc: terminate main loop if FOC control loop terminated
new 171a3c4 industry/foc/qenco: add support for encoder index
new 15b66aa examples/foc: add Qenco support
The 2 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails. The revisions
listed as "add" were already present in the repository and have only
been added to this reference.
Summary of changes:
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 +++
industry/foc/fixed16/foc_ang_qenco.c | 44 ++++++++++++++++++++------
industry/foc/float/foc_ang_qenco.c | 35 ++++++++++++++++++---
8 files changed, 232 insertions(+), 19 deletions(-)
[incubator-nuttx-apps] 01/02: industry/foc/qenco: add support for
encoder index
Posted by xi...@apache.org.
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 171a3c41df4df3fb35f21ab4918b5603a7f51909
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Nov 6 18:44:13 2021 +0100
industry/foc/qenco: add support for encoder index
---
industry/foc/fixed16/foc_ang_qenco.c | 44 ++++++++++++++++++++++++++++--------
industry/foc/float/foc_ang_qenco.c | 35 ++++++++++++++++++++++++----
2 files changed, 65 insertions(+), 14 deletions(-)
diff --git a/industry/foc/fixed16/foc_ang_qenco.c b/industry/foc/fixed16/foc_ang_qenco.c
index 2b60f1d..af5b907 100644
--- a/industry/foc/fixed16/foc_ang_qenco.c
+++ b/industry/foc/fixed16/foc_ang_qenco.c
@@ -46,6 +46,7 @@ struct foc_qenco_b16_s
{
int fd;
int32_t pos;
+ int32_t offset;
b16_t one_by_posmax;
b16_t dir;
b16_t angle;
@@ -201,14 +202,36 @@ static int foc_angle_qe_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
goto errout;
}
+ /* Set the encoder index position to 0 */
+
+ ret = ioctl(qe->fd, QEIOC_SETINDEX, (unsigned long)(0));
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: QEIOC_SETINDEX failed, errno=%d\n", errno);
+ goto errout;
+ }
+
+ /* Reset encoder position */
+
+ ret = ioctl(qe->fd, QEIOC_RESET, 0);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
+ goto errout;
+ }
+
/* Get helpers */
- qe->one_by_posmax = b16divb16(b16ONE, qe->cfg.posmax);
+ qe->one_by_posmax = b16divi(b16ONE, qe->cfg.posmax);
/* Initialize with CW direction */
qe->dir = DIR_CW_B16;
+ /* Reset offset */
+
+ qe->offset = 0;
+
errout:
return ret;
}
@@ -236,12 +259,13 @@ static int foc_angle_qe_zero_b16(FAR foc_angle_b16_t *h)
DEBUGASSERT(h->data);
qe = h->data;
- /* Reset encoder position */
+ /* Get the zero offset position from encoder */
- ret = ioctl(qe->fd, QEIOC_RESET, 0);
+ ret = ioctl(qe->fd, QEIOC_POSITION,
+ (unsigned long)((uintptr_t)&qe->offset));
if (ret < 0)
{
- FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
+ FOCLIBERR("ERROR: QEIOC_POSITION failed, errno=%d\n", errno);
goto errout;
}
@@ -305,6 +329,7 @@ static int foc_angle_qe_run_b16(FAR foc_angle_b16_t *h,
int ret = OK;
b16_t tmp1 = 0;
b16_t tmp2 = 0;
+ b16_t tmp3 = 0;
DEBUGASSERT(h);
@@ -324,15 +349,16 @@ static int foc_angle_qe_run_b16(FAR foc_angle_b16_t *h,
/* Get mechanical angle */
- tmp1 = b16muli(qe->dir, qe->pos);
- tmp2 = b16mulb16(qe->one_by_posmax, MOTOR_ANGLE_E_MAX_B16);
+ tmp1 = (qe->pos - qe->offset);
+ tmp2 = b16muli(qe->dir, tmp1);
+ tmp3 = b16mulb16(qe->one_by_posmax, MOTOR_ANGLE_M_MAX_B16);
- qe->angle = b16mulb16(tmp1, tmp2);
+ qe->angle = b16mulb16(tmp2, tmp3);
/* Normalize angle */
- angle_norm_2pi_b16(&qe->angle, MOTOR_ANGLE_E_MIN_B16,
- MOTOR_ANGLE_E_MAX_B16);
+ angle_norm_2pi_b16(&qe->angle, MOTOR_ANGLE_M_MIN_B16,
+ MOTOR_ANGLE_M_MAX_B16);
/* Copy data */
diff --git a/industry/foc/float/foc_ang_qenco.c b/industry/foc/float/foc_ang_qenco.c
index 95c1c8c..cfde176 100644
--- a/industry/foc/float/foc_ang_qenco.c
+++ b/industry/foc/float/foc_ang_qenco.c
@@ -46,6 +46,7 @@ struct foc_qenco_f32_s
{
int fd;
int32_t pos;
+ int32_t offset;
float one_by_posmax;
float dir;
float angle;
@@ -201,6 +202,24 @@ static int foc_angle_qe_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
goto errout;
}
+ /* Set the encoder index position to 0 */
+
+ ret = ioctl(qe->fd, QEIOC_SETINDEX, (unsigned long)(0));
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: QEIOC_SETINDEX failed, errno=%d\n", errno);
+ goto errout;
+ }
+
+ /* Reset encoder position */
+
+ ret = ioctl(qe->fd, QEIOC_RESET, 0);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
+ goto errout;
+ }
+
/* Get helpers */
qe->one_by_posmax = (1.0f / qe->cfg.posmax);
@@ -209,6 +228,10 @@ static int foc_angle_qe_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
qe->dir = DIR_CW;
+ /* Reset offset */
+
+ qe->offset = 0.0f;
+
errout:
return ret;
}
@@ -236,12 +259,13 @@ static int foc_angle_qe_zero_f32(FAR foc_angle_f32_t *h)
DEBUGASSERT(h->data);
qe = h->data;
- /* Reset encoder position */
+ /* Get the zero offset position from encoder */
- ret = ioctl(qe->fd, QEIOC_RESET, 0);
+ ret = ioctl(qe->fd, QEIOC_POSITION,
+ (unsigned long)((uintptr_t)&qe->offset));
if (ret < 0)
{
- FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
+ FOCLIBERR("ERROR: QEIOC_POSITION failed, errno=%d\n", errno);
goto errout;
}
@@ -322,11 +346,12 @@ static int foc_angle_qe_run_f32(FAR foc_angle_f32_t *h,
/* Get mechanical angle */
- qe->angle = qe->dir * qe->pos * qe->one_by_posmax * MOTOR_ANGLE_E_MAX;
+ qe->angle = (qe->dir * (qe->pos - qe->offset) *
+ qe->one_by_posmax * MOTOR_ANGLE_M_MAX);
/* Normalize angle */
- angle_norm_2pi(&qe->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX);
+ angle_norm_2pi(&qe->angle, MOTOR_ANGLE_M_MIN, MOTOR_ANGLE_M_MAX);
/* Copy data */
[incubator-nuttx-apps] 02/02: examples/foc: add Qenco support
Posted by xi...@apache.org.
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 */