diff --git a/include/industry/foc/fixed16/foc_angle.h b/include/industry/foc/fixed16/foc_angle.h index 4fd373790..3a9190be3 100644 --- a/include/industry/foc/fixed16/foc_angle.h +++ b/include/industry/foc/fixed16/foc_angle.h @@ -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 ****************************************************************************/ diff --git a/include/industry/foc/float/foc_angle.h b/include/industry/foc/float/foc_angle.h index 6fcb70042..0d15de5b7 100644 --- a/include/industry/foc/float/foc_angle.h +++ b/include/industry/foc/float/foc_angle.h @@ -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 ****************************************************************************/ diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig index aff1209d7..44e024763 100644 --- a/industry/foc/Kconfig +++ b/industry/foc/Kconfig @@ -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 diff --git a/industry/foc/Makefile b/industry/foc/Makefile index 0e9ba2c69..a1b4ee65c 100644 --- a/industry/foc/Makefile +++ b/industry/foc/Makefile @@ -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 diff --git a/industry/foc/fixed16/foc_ang_qenco.c b/industry/foc/fixed16/foc_ang_qenco.c new file mode 100644 index 000000000..2b60f1daf --- /dev/null +++ b/industry/foc/fixed16/foc_ang_qenco.c @@ -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 + +#include + +#include +#include +#include +#include + +#include + +#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; +} diff --git a/industry/foc/float/foc_ang_qenco.c b/industry/foc/float/foc_ang_qenco.c new file mode 100644 index 000000000..95c1c8ca5 --- /dev/null +++ b/industry/foc/float/foc_ang_qenco.c @@ -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 + +#include + +#include +#include +#include +#include + +#include + +#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; +}