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

industry/foc: add support for qenco

Browse Source
This commit is contained in:
raiden00pl 2021-10-29 13:02:36 +02:00 committed by Xiang Xiao
parent df1638a3aa
commit 5b52247625
6 changed files with 727 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
include/industry/foc/fixed16/foc_angle.h
View File

@ -106,6 +106,16 @@ struct foc_openloop_cfg_b16_s
};
#endif
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder configuration data */
struct foc_qenco_cfg_b16_s
{
FAR char *devpath;
uint32_t posmax;
};
#endif
/****************************************************************************
* Public Data
****************************************************************************/
@ -116,6 +126,12 @@ struct foc_openloop_cfg_b16_s
extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16;
#endif
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder angle operations (fixed16) */
extern struct foc_angle_ops_b16_s g_foc_angle_qe_b16;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/

16
include/industry/foc/float/foc_angle.h
View File

@ -106,6 +106,16 @@ struct foc_openloop_cfg_f32_s
};
#endif /* CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP */
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder configuration data */
struct foc_qenco_cfg_f32_s
{
FAR char *devpath;
uint32_t posmax;
};
#endif
/****************************************************************************
* Public Data
****************************************************************************/
@ -116,6 +126,12 @@ struct foc_openloop_cfg_f32_s
extern struct foc_angle_ops_f32_s g_foc_angle_ol_f32;
#endif
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder angle operations (float) */
extern struct foc_angle_ops_f32_s g_foc_angle_qe_f32;
#endif
/****************************************************************************
* Public Function Prototypes
****************************************************************************/

7
industry/foc/Kconfig
View File

@ -58,6 +58,13 @@ config INDUSTRY_FOC_ANGLE_OPENLOOP
---help---
Enable support for open-loop angle handler
config INDUSTRY_FOC_ANGLE_QENCO
bool "FOC angle quadrature encoder handler"
default n
depends on SENSORS_QENCODER
---help---
Enable support for quadrature encoder angle handler
config INDUSTRY_FOC_CONTROL_PI
bool "FOC PI controller"
default y

6
industry/foc/Makefile
View File

@ -36,6 +36,9 @@ CSRCS += float/foc_routine.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += float/foc_ang_openloop.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y)
CSRCS += float/foc_ang_qenco.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
CSRCS += float/foc_cordic.c
endif
@ -72,6 +75,9 @@ CSRCS += fixed16/foc_routine.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += fixed16/foc_ang_openloop.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y)
CSRCS += fixed16/foc_ang_qenco.c
endif
ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
CSRCS += fixed16/foc_cordic.c
endif

344
industry/foc/fixed16/foc_ang_qenco.c Normal file
View File

@ -0,0 +1,344 @@
/****************************************************************************
* apps/industry/foc/fixed16/foc_ang_qenco.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <fcntl.h>
#include <nuttx/sensors/qencoder.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_angle.h"
/****************************************************************************
* Private Data Types
****************************************************************************/
/* Qenco private data */
struct foc_qenco_b16_s
{
int fd;
int32_t pos;
b16_t one_by_posmax;
b16_t dir;
b16_t angle;
struct foc_qenco_cfg_b16_s cfg;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_angle_qe_init_b16(FAR foc_angle_b16_t *h);
static void foc_angle_qe_deinit_b16(FAR foc_angle_b16_t *h);
static int foc_angle_qe_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
static int foc_angle_qe_zero_b16(FAR foc_angle_b16_t *h);
static int foc_angle_qe_dir_b16(FAR foc_angle_b16_t *h, b16_t dir);
static int foc_angle_qe_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC angle fixed16 interface */
struct foc_angle_ops_b16_s g_foc_angle_qe_b16 =
{
.init = foc_angle_qe_init_b16,
.deinit = foc_angle_qe_deinit_b16,
.cfg = foc_angle_qe_cfg_b16,
.zero = foc_angle_qe_zero_b16,
.dir = foc_angle_qe_dir_b16,
.run = foc_angle_qe_run_b16,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_qe_init_b16
*
* Description:
* Initialize qenco the FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_qe_init_b16(FAR foc_angle_b16_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect angle data */
h->data = zalloc(sizeof(struct foc_qenco_b16_s));
if (h->data == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_deinit_b16
*
* Description:
* De-initialize qenco the FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static void foc_angle_qe_deinit_b16(FAR foc_angle_b16_t *h)
{
FAR struct foc_qenco_b16_s *qe = NULL;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
if (h->data)
{
/* Close file */
if (qe->fd > 0)
{
close(qe->fd);
}
/* Free angle data */
free(h->data);
}
}
/****************************************************************************
* Name: foc_angle_qe_cfg_b16
*
* Description:
* Configure the qenco FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
* (struct foc_qenco_b16_s)
*
****************************************************************************/
static int foc_angle_qe_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
{
FAR struct foc_qenco_b16_s *qe = NULL;
int ret = OK;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Copy configuration */
memcpy(&qe->cfg, cfg, sizeof(struct foc_qenco_cfg_b16_s));
/* Open qenco device */
qe->fd = open(qe->cfg.devpath, O_RDONLY);
if (qe->fd <= 0)
{
FOCLIBERR("ERROR: failed to open %s, errno=%d\n",
qe->cfg.devpath, errno);
ret = -errno;
goto errout;
}
/* Configure the maximum encoder position */
ret = ioctl(qe->fd, QEIOC_SETPOSMAX, (unsigned long)(qe->cfg.posmax));
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_SETPOSMAX failed, errno=%d\n", errno);
goto errout;
}
/* Get helpers */
qe->one_by_posmax = b16divb16(b16ONE, qe->cfg.posmax);
/* Initialize with CW direction */
qe->dir = DIR_CW_B16;
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_zero_b16
*
* Description:
* Zero the qenco FOC angle handler (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_qe_zero_b16(FAR foc_angle_b16_t *h)
{
FAR struct foc_qenco_b16_s *qe = NULL;
int ret = OK;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Reset encoder position */
ret = ioctl(qe->fd, QEIOC_RESET, 0);
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
goto errout;
}
/* Reset data */
qe->angle = 0;
qe->pos = 0;
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_dir_b16
*
* Description:
* Set the qenco FOC angle handler direction (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* dir - sensor direction (1 if normal -1 if inverted)
*
****************************************************************************/
static int foc_angle_qe_dir_b16(FAR foc_angle_b16_t *h, b16_t dir)
{
FAR struct foc_qenco_b16_s *qe = NULL;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Configure direction */
qe->dir = dir;
return OK;
}
/****************************************************************************
* Name: foc_angle_qe_run_b16
*
* Description:
* Process the qenco FOC angle data (fixed16)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
static int foc_angle_qe_run_b16(FAR foc_angle_b16_t *h,
FAR struct foc_angle_in_b16_s *in,
FAR struct foc_angle_out_b16_s *out)
{
FAR struct foc_qenco_b16_s *qe = NULL;
int ret = OK;
b16_t tmp1 = 0;
b16_t tmp2 = 0;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Get the positions from encoder */
ret = ioctl(qe->fd, QEIOC_POSITION, (unsigned long)((uintptr_t)&qe->pos));
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_POSITION failed, errno=%d\n", errno);
goto errout;
}
/* Get mechanical angle */
tmp1 = b16muli(qe->dir, qe->pos);
tmp2 = b16mulb16(qe->one_by_posmax, MOTOR_ANGLE_E_MAX_B16);
qe->angle = b16mulb16(tmp1, tmp2);
/* Normalize angle */
angle_norm_2pi_b16(&qe->angle, MOTOR_ANGLE_E_MIN_B16,
MOTOR_ANGLE_E_MAX_B16);
/* Copy data */
out->type = FOC_ANGLE_TYPE_MECH;
out->angle = qe->angle;
errout:
return ret;
}

338
industry/foc/float/foc_ang_qenco.c Normal file
View File

@ -0,0 +1,338 @@
/****************************************************************************
* apps/industry/foc/float/foc_ang_qenco.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <fcntl.h>
#include <nuttx/sensors/qencoder.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/float/foc_angle.h"
/****************************************************************************
* Private Data Types
****************************************************************************/
/* Qenco private data */
struct foc_qenco_f32_s
{
int fd;
int32_t pos;
float one_by_posmax;
float dir;
float angle;
struct foc_qenco_cfg_f32_s cfg;
};
/****************************************************************************
* Private Function Prototypes
****************************************************************************/
static int foc_angle_qe_init_f32(FAR foc_angle_f32_t *h);
static void foc_angle_qe_deinit_f32(FAR foc_angle_f32_t *h);
static int foc_angle_qe_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
static int foc_angle_qe_zero_f32(FAR foc_angle_f32_t *h);
static int foc_angle_qe_dir_f32(FAR foc_angle_f32_t *h, float dir);
static int foc_angle_qe_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out);
/****************************************************************************
* Public Data
****************************************************************************/
/* FOC angle float interface */
struct foc_angle_ops_f32_s g_foc_angle_qe_f32 =
{
.init = foc_angle_qe_init_f32,
.deinit = foc_angle_qe_deinit_f32,
.cfg = foc_angle_qe_cfg_f32,
.zero = foc_angle_qe_zero_f32,
.dir = foc_angle_qe_dir_f32,
.run = foc_angle_qe_run_f32,
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_angle_qe_init_f32
*
* Description:
* Initialize qenco the FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_qe_init_f32(FAR foc_angle_f32_t *h)
{
int ret = OK;
DEBUGASSERT(h);
/* Connect angle data */
h->data = zalloc(sizeof(struct foc_qenco_f32_s));
if (h->data == NULL)
{
ret = -ENOMEM;
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_deinit_f32
*
* Description:
* De-initialize qenco the FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static void foc_angle_qe_deinit_f32(FAR foc_angle_f32_t *h)
{
FAR struct foc_qenco_f32_s *qe = NULL;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
if (h->data)
{
/* Close file */
if (qe->fd > 0)
{
close(qe->fd);
}
/* Free angle data */
free(h->data);
}
}
/****************************************************************************
* Name: foc_angle_qe_cfg_f32
*
* Description:
* Configure the qenco FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* cfg - pointer to angle handler configuration data
* (struct foc_qenco_f32_s)
*
****************************************************************************/
static int foc_angle_qe_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
{
FAR struct foc_qenco_f32_s *qe = NULL;
int ret = OK;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Copy configuration */
memcpy(&qe->cfg, cfg, sizeof(struct foc_qenco_cfg_f32_s));
/* Open qenco device */
qe->fd = open(qe->cfg.devpath, O_RDONLY);
if (qe->fd <= 0)
{
FOCLIBERR("ERROR: failed to open %s, errno=%d\n",
qe->cfg.devpath, errno);
ret = -errno;
goto errout;
}
/* Configure the maximum encoder position */
ret = ioctl(qe->fd, QEIOC_SETPOSMAX, (unsigned long)(qe->cfg.posmax));
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_SETPOSMAX failed, errno=%d\n", errno);
goto errout;
}
/* Get helpers */
qe->one_by_posmax = (1.0f / qe->cfg.posmax);
/* Initialize with CW direction */
qe->dir = DIR_CW;
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_zero_f32
*
* Description:
* Zero the qenco FOC angle handler (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
*
****************************************************************************/
static int foc_angle_qe_zero_f32(FAR foc_angle_f32_t *h)
{
FAR struct foc_qenco_f32_s *qe = NULL;
int ret = OK;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Reset encoder position */
ret = ioctl(qe->fd, QEIOC_RESET, 0);
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_RESET failed, errno=%d\n", errno);
goto errout;
}
/* Reset data */
qe->angle = 0.0f;
qe->pos = 0;
errout:
return ret;
}
/****************************************************************************
* Name: foc_angle_qe_dir_f32
*
* Description:
* Set the qenco FOC angle handler direction (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* dir - sensor direction (1 if normal -1 if inverted)
*
****************************************************************************/
static int foc_angle_qe_dir_f32(FAR foc_angle_f32_t *h, float dir)
{
FAR struct foc_qenco_f32_s *qe = NULL;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Configure direction */
qe->dir = dir;
return OK;
}
/****************************************************************************
* Name: foc_angle_qe_run_f32
*
* Description:
* Process the qenco FOC angle data (float32)
*
* Input Parameter:
* h - pointer to FOC angle handler
* in - pointer to FOC angle handler input data
* out - pointer to FOC angle handler output data
*
****************************************************************************/
static int foc_angle_qe_run_f32(FAR foc_angle_f32_t *h,
FAR struct foc_angle_in_f32_s *in,
FAR struct foc_angle_out_f32_s *out)
{
FAR struct foc_qenco_f32_s *qe = NULL;
int ret = OK;
DEBUGASSERT(h);
/* Get qenco data */
DEBUGASSERT(h->data);
qe = h->data;
/* Get the positions from encoder */
ret = ioctl(qe->fd, QEIOC_POSITION, (unsigned long)((uintptr_t)&qe->pos));
if (ret < 0)
{
FOCLIBERR("ERROR: QEIOC_POSITION failed, errno=%d\n", errno);
goto errout;
}
/* Get mechanical angle */
qe->angle = qe->dir * qe->pos * qe->one_by_posmax * MOTOR_ANGLE_E_MAX;
/* Normalize angle */
angle_norm_2pi(&qe->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX);
/* Copy data */
out->type = FOC_ANGLE_TYPE_MECH;
out->angle = qe->angle;
errout:
return ret;
}