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:10 UTC
[incubator-nuttx-apps] 01/02: industry/foc/qenco: add support for
encoder index
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 */