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 2022/02/16 15:49:18 UTC
[incubator-nuttx-apps] branch master updated: industry/foc/float: add velocity observers support
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
The following commit(s) were added to refs/heads/master by this push:
new 5ef9d36 industry/foc/float: add velocity observers support
5ef9d36 is described below
commit 5ef9d3630bf485ae5a8d353f1575156b8c8e274d
Author: raiden00pl <ra...@railab.me>
AuthorDate: Wed Feb 16 12:10:19 2022 +0100
industry/foc/float: add velocity observers support
---
include/industry/foc/float/foc_velocity.h | 38 ++++
industry/foc/Kconfig | 12 ++
industry/foc/Makefile | 6 +
industry/foc/float/foc_vel_odiv.c | 279 ++++++++++++++++++++++++++++++
industry/foc/float/foc_vel_opll.c | 277 +++++++++++++++++++++++++++++
5 files changed, 612 insertions(+)
diff --git a/include/industry/foc/float/foc_velocity.h b/include/industry/foc/float/foc_velocity.h
index 221ca0c..971a9e9 100644
--- a/include/industry/foc/float/foc_velocity.h
+++ b/include/industry/foc/float/foc_velocity.h
@@ -95,6 +95,44 @@ struct foc_velocity_f32_s
FAR void *data;
};
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV
+/* Velocity DIV observer */
+
+struct foc_vel_div_f32_cfg_s
+{
+ uint8_t samples;
+ float filter;
+ float per;
+};
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL
+/* Velocity PLL observer */
+
+struct foc_vel_pll_f32_cfg_s
+{
+ float kp;
+ float ki;
+ float per;
+};
+#endif
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV
+/* Velocity DIV observer (float) */
+
+extern struct foc_velocity_ops_f32_s g_foc_velocity_odiv_f32;
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL
+/* Velocity PLL observer (float) */
+
+extern struct foc_velocity_ops_f32_s g_foc_velocity_opll_f32;
+#endif
+
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index 790c76a..896b15b 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -137,4 +137,16 @@ config INDUSTRY_FOC_IDENT
---help---
Enable support for motor identification routine (phase resistance and phase inductance)
+config INDUSTRY_FOC_VELOCITY_ODIV
+ bool "FOC velocity DIV observer"
+ default n
+ ---help---
+ Enable support for the velocity DIV observer
+
+config INDUSTRY_FOC_VELOCITY_OPLL
+ bool "FOC velocity PLL observer"
+ default n
+ ---help---
+ Enable support for the velocity PLL observer
+
endif
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 6fb1780..98fedb9 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -63,6 +63,12 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
CSRCS += float/foc_ident.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_ODIV),y)
+CSRCS += float/foc_vel_odiv.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL),y)
+CSRCS += float/foc_vel_opll.c
+endif
endif
diff --git a/industry/foc/float/foc_vel_odiv.c b/industry/foc/float/foc_vel_odiv.c
new file mode 100644
index 0000000..41604f6
--- /dev/null
+++ b/industry/foc/float/foc_vel_odiv.c
@@ -0,0 +1,279 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_vel_odiv.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 <assert.h>
+#include <errno.h>
+#include <stdlib.h>
+
+#include <dsp.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+#include "industry/foc/float/foc_velocity.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Div private data */
+
+struct foc_div_f32_s
+{
+ struct foc_vel_div_f32_cfg_s cfg;
+ struct motor_sobserver_div_f32_s data;
+ struct motor_sobserver_f32_s o;
+ float dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_velocity_div_init_f32(FAR foc_velocity_f32_t *h);
+static void foc_velocity_div_deinit_f32(FAR foc_velocity_f32_t *h);
+static int foc_velocity_div_cfg_f32(FAR foc_velocity_f32_t *h,
+ FAR void *cfg);
+static int foc_velocity_div_zero_f32(FAR foc_velocity_f32_t *h);
+static int foc_velocity_div_dir_f32(FAR foc_velocity_f32_t *h, float dir);
+static int foc_velocity_div_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC velocity float interface */
+
+struct foc_velocity_ops_f32_s g_foc_velocity_odiv_f32 =
+{
+ .init = foc_velocity_div_init_f32,
+ .deinit = foc_velocity_div_deinit_f32,
+ .cfg = foc_velocity_div_cfg_f32,
+ .zero = foc_velocity_div_zero_f32,
+ .dir = foc_velocity_div_dir_f32,
+ .run = foc_velocity_div_run_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_div_init_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_init_f32(FAR foc_velocity_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect velocity data */
+
+ h->data = zalloc(sizeof(struct foc_div_f32_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_deinit_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static void foc_velocity_div_deinit_f32(FAR foc_velocity_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free velocity data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_cfg_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ * (struct foc_div_f32_s)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_div_f32_s *div = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&div->cfg, cfg, sizeof(struct foc_vel_div_f32_cfg_s));
+
+ /* Configure observer */
+
+ motor_sobserver_div_init(&div->data,
+ div->cfg.samples,
+ div->cfg.filter,
+ div->cfg.per);
+
+ motor_sobserver_init(&div->o, &div->data, div->cfg.per);
+
+ /* Initialize with CW direction */
+
+ div->dir = DIR_CW;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_zero_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_zero_f32(FAR foc_velocity_f32_t *h)
+{
+ FAR struct foc_div_f32_s *div = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Reinitialize observer */
+
+ motor_sobserver_div_init(&div->data,
+ div->cfg.samples,
+ div->cfg.filter,
+ div->cfg.per);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_dir_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_dir_f32(FAR foc_velocity_f32_t *h, float dir)
+{
+ FAR struct foc_div_f32_s *div = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Set direction */
+
+ div->dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_run_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * in - pointer to FOC velocity handler input data
+ * out - pointer to FOC velocity handler output data
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out)
+{
+ FAR struct foc_div_f32_s *div = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Run observer */
+
+ motor_sobserver_div(&div->o, in->angle);
+
+ /* Copy data */
+
+ out->velocity = div->dir * motor_sobserver_speed_get(&div->o);
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_vel_opll.c b/industry/foc/float/foc_vel_opll.c
new file mode 100644
index 0000000..8b20eba
--- /dev/null
+++ b/industry/foc/float/foc_vel_opll.c
@@ -0,0 +1,277 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_vel_opll.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 <assert.h>
+#include <errno.h>
+#include <stdlib.h>
+
+#include <dsp.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+#include "industry/foc/float/foc_velocity.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* PLL observer private data */
+
+struct foc_pll_f32_s
+{
+ struct foc_vel_pll_f32_cfg_s cfg;
+ struct motor_sobserver_pll_f32_s data;
+ struct motor_sobserver_f32_s o;
+ float dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_velocity_pll_init_f32(FAR foc_velocity_f32_t *h);
+static void foc_velocity_pll_deinit_f32(FAR foc_velocity_f32_t *h);
+static int foc_velocity_pll_cfg_f32(FAR foc_velocity_f32_t *h,
+ FAR void *cfg);
+static int foc_velocity_pll_zero_f32(FAR foc_velocity_f32_t *h);
+static int foc_velocity_pll_dir_f32(FAR foc_velocity_f32_t *h, float dir);
+static int foc_velocity_pll_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC velocity float interface */
+
+struct foc_velocity_ops_f32_s g_foc_velocity_opll_f32 =
+{
+ .init = foc_velocity_pll_init_f32,
+ .deinit = foc_velocity_pll_deinit_f32,
+ .cfg = foc_velocity_pll_cfg_f32,
+ .zero = foc_velocity_pll_zero_f32,
+ .dir = foc_velocity_pll_dir_f32,
+ .run = foc_velocity_pll_run_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_pll_init_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_init_f32(FAR foc_velocity_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect velocity data */
+
+ h->data = zalloc(sizeof(struct foc_pll_f32_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_deinit_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static void foc_velocity_pll_deinit_f32(FAR foc_velocity_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free velocity data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_cfg_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ * (struct foc_pll_f32_s)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_pll_f32_s *pll = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&pll->cfg, cfg, sizeof(struct foc_vel_pll_f32_cfg_s));
+
+ /* Configure observer */
+
+ motor_sobserver_pll_init(&pll->data,
+ pll->cfg.kp,
+ pll->cfg.ki);
+
+ motor_sobserver_init(&pll->o, &pll->data, pll->cfg.per);
+
+ /* Initialize with CW direction */
+
+ pll->dir = DIR_CW;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_zero_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_zero_f32(FAR foc_velocity_f32_t *h)
+{
+ FAR struct foc_pll_f32_s *pll = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Reinitialize observer */
+
+ motor_sobserver_pll_init(&pll->data,
+ pll->cfg.kp,
+ pll->cfg.ki);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_dir_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_dir_f32(FAR foc_velocity_f32_t *h, float dir)
+{
+ FAR struct foc_pll_f32_s *pll = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Set direction */
+
+ pll->dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_run_f32
+ *
+ * Description:
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * in - pointer to FOC velocity handler input data
+ * out - pointer to FOC velocity handler output data
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out)
+{
+ FAR struct foc_pll_f32_s *pll = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Run observer */
+
+ motor_sobserver_pll(&pll->o, in->angle);
+
+ /* Copy data */
+
+ out->velocity = pll->dir * motor_sobserver_speed_get(&pll->o);
+
+ return OK;
+}