sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

industry/foc/qenco: add support for encoder index

Browse Source
This commit is contained in:
raiden00pl 2021-11-06 18:44:13 +01:00 committed by Xiang Xiao
parent f38d7d2ed3
commit 171a3c41df
2 changed files with 65 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
industry/foc/fixed16/foc_ang_qenco.c
View File

@ -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 */

35
industry/foc/float/foc_ang_qenco.c
View File

@ -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 */