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/10/30 06:18:16 UTC
[incubator-nuttx-apps] branch master updated (f5e40d3 -> f009d68)
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git.
from f5e40d3 fsutils/mkgpt: support gpt partition format
new 084cfa4 industry/foc: add general FOC routine handler
new f2fc2af industry/foc: add sensor alignment routine
new f009d68 industry/foc: add motor identification routine
The 3 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails. The revisions
listed as "add" were already present in the repository and have only
been added to this reference.
Summary of changes:
.../industry/foc/fixed16/foc_align.h | 53 +-
.../industry/foc/fixed16/foc_ident.h | 39 +-
.../foc/fixed16/{foc_angle.h => foc_routine.h} | 110 +--
.../industry/foc/float/foc_align.h | 53 +-
.../industry/foc/float/foc_ident.h | 39 +-
.../foc/float/{foc_angle.h => foc_routine.h} | 110 +--
industry/foc/Kconfig | 12 +
industry/foc/Makefile | 14 +
industry/foc/fixed16/foc_align.c | 825 +++++++++++++++++++++
industry/foc/fixed16/foc_ident.c | 608 +++++++++++++++
.../{float/foc_model.c => fixed16/foc_routine.c} | 117 ++-
industry/foc/float/foc_align.c | 825 +++++++++++++++++++++
industry/foc/float/foc_ident.c | 601 +++++++++++++++
industry/foc/float/{foc_model.c => foc_routine.c} | 117 ++-
14 files changed, 3235 insertions(+), 288 deletions(-)
copy examples/foc/foc_device.h => include/industry/foc/fixed16/foc_align.h (57%)
copy examples/foc/foc_device.h => include/industry/foc/fixed16/foc_ident.h (60%)
copy include/industry/foc/fixed16/{foc_angle.h => foc_routine.h} (53%)
copy examples/foc/foc_device.h => include/industry/foc/float/foc_align.h (57%)
copy examples/foc/foc_device.h => include/industry/foc/float/foc_ident.h (60%)
copy include/industry/foc/float/{foc_angle.h => foc_routine.h} (53%)
create mode 100644 industry/foc/fixed16/foc_align.c
create mode 100644 industry/foc/fixed16/foc_ident.c
copy industry/foc/{float/foc_model.c => fixed16/foc_routine.c} (57%)
create mode 100644 industry/foc/float/foc_align.c
create mode 100644 industry/foc/float/foc_ident.c
copy industry/foc/float/{foc_model.c => foc_routine.c} (57%)
[incubator-nuttx-apps] 02/03: industry/foc: add sensor alignment
routine
Posted by xi...@apache.org.
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 f2fc2af8e3e5c69b819884c33c45bf6cf3c1d2fe
Author: raiden00pl <ra...@railab.me>
AuthorDate: Fri Oct 29 13:33:45 2021 +0200
industry/foc: add sensor alignment routine
---
include/industry/foc/fixed16/foc_align.h | 79 +++
include/industry/foc/float/foc_align.h | 79 +++
industry/foc/Kconfig | 6 +
industry/foc/Makefile | 6 +
industry/foc/fixed16/foc_align.c | 825 +++++++++++++++++++++++++++++++
industry/foc/float/foc_align.c | 825 +++++++++++++++++++++++++++++++
6 files changed, 1820 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_align.h b/include/industry/foc/fixed16/foc_align.h
new file mode 100644
index 0000000..27403df
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_align.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_align.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FIXED16_FOC_ALIGN_H
+#define __INDUSTRY_FOC_FIXED16_FOC_ALIGN_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#include "industry/foc/fixed16/foc_routine.h"
+#include "industry/foc/fixed16/foc_angle.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Align routine callbacks */
+
+struct foc_routine_align_cb_b16_s
+{
+ /* Private data for callbacks */
+
+ FAR void *priv;
+
+ /* Align angle zero callback */
+
+ CODE int (*zero)(FAR void *priv, b16_t offset);
+
+ /* Align angle direction callback */
+
+ CODE int (*dir)(FAR void *priv, b16_t dir);
+};
+
+/* Align routine configuration */
+
+struct foc_routine_align_cfg_b16_s
+{
+ struct foc_routine_align_cb_b16_s cb; /* Align routine callbacks */
+ b16_t volt; /* Align voltage */
+ int offset_steps; /* Offset alignment steps */
+};
+
+/* Align routine final data */
+
+struct foc_routine_aling_final_b16_s
+{
+ b16_t dir; /* Angle sensor direction */
+ b16_t offset; /* Angle sensor offset */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_b16_s g_foc_routine_align_b16;
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_ALIGN_H */
diff --git a/include/industry/foc/float/foc_align.h b/include/industry/foc/float/foc_align.h
new file mode 100644
index 0000000..f0e3035
--- /dev/null
+++ b/include/industry/foc/float/foc_align.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_align.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FLOAT_FOC_ALIGN_H
+#define __INDUSTRY_FOC_FLOAT_FOC_ALIGN_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#include "industry/foc/float/foc_routine.h"
+#include "industry/foc/float/foc_angle.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Align routine callbacks */
+
+struct foc_routine_align_cb_f32_s
+{
+ /* Private data for callbacks */
+
+ FAR void *priv;
+
+ /* Align angle zero callback */
+
+ CODE int (*zero)(FAR void *priv, float offset);
+
+ /* Align angle direction callback */
+
+ CODE int (*dir)(FAR void *priv, float dir);
+};
+
+/* Align routine configuration */
+
+struct foc_routine_align_cfg_f32_s
+{
+ struct foc_routine_align_cb_f32_s cb; /* Align routine callbacks */
+ float volt; /* Align voltage */
+ int offset_steps; /* Offset alignment steps */
+};
+
+/* Align routine final data */
+
+struct foc_routine_aling_final_f32_s
+{
+ float dir; /* Angle sensor direction */
+ float offset; /* Angle sensor offset */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_f32_s g_foc_routine_align_f32;
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_ALIGN_H */
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index d26c68d..5124cea 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -81,4 +81,10 @@ config INDUSTRY_FOC_HAVE_MODEL
bool
default n
+config INDUSTRY_FOC_ALIGN
+ bool "FOC sensor alignment routine"
+ default n
+ ---help---
+ Enable support for angle sensor alignment routine (zero offset + direction)
+
endif
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 60f00ca..83489f8 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -51,6 +51,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
CSRCS += float/foc_model_pmsm.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
+CSRCS += float/foc_align.c
+endif
endif
@@ -81,6 +84,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
CSRCS += fixed16/foc_model_pmsm.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
+CSRCS += fixed16/foc_align.c
+endif
endif
diff --git a/industry/foc/fixed16/foc_align.c b/industry/foc/fixed16/foc_align.c
new file mode 100644
index 0000000..43dcdb5
--- /dev/null
+++ b/industry/foc/fixed16/foc_align.c
@@ -0,0 +1,825 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_align.c
+ * This file implements angle sensor alignment routine for fixed16
+ *
+ * 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 "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/fixed16/foc_align.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Align to -90deg = 270deg */
+
+#define FOC_ALIGN_ANGLE (b16muli(b16HALFPI, 3))
+
+/* Direction alignment */
+
+#define ALIGN_DIR_ANGLE_STEP (ftob16(0.001f))
+#define ALIGN_DIR_ANGLE_HOLD_0 (ftob16(0.0f))
+#define ALIGN_DIR_ANGLE_HOLD_1 (b16idiv(b16PI, 3))
+#define ALIGN_DIR_ANGLE_HOLD_2 (b16muli(b16idiv(b16PI, 3), 2))
+#define ALIGN_DIR_ANGLE_HOLD_3 (b16PI)
+#define ALIGN_DIR_ANGLE_HOLD_4 (b16muli(b16idiv(b16PI, 3), 4))
+#define ALIGN_DIR_HOLD_CNTR (10)
+
+/* IDLE steps */
+
+#define IDLE_STEPS (500)
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Align stage */
+
+enum foc_align_run_stage_e
+{
+ FOC_ALIGN_RUN_INIT,
+ FOC_ALIGN_RUN_OFFSET,
+ FOC_ALIGN_RUN_DIR,
+ FOC_ALIGN_RUN_IDLE,
+ FOC_ALIGN_RUN_DONE
+};
+
+/* Align routine private data */
+
+struct foc_align_b16_s
+{
+ struct foc_routine_align_cfg_b16_s cfg;
+ struct foc_routine_aling_final_b16_s final;
+ int cntr;
+ int stage;
+ int dir_step;
+ b16_t dir_angle;
+ b16_t angle_last;
+ b16_t angle_now;
+ b16_t diff_cw;
+ b16_t diff_ccw;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_align_init_b16(FAR foc_routine_b16_t *r);
+void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r);
+int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
+int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out);
+int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_b16_s g_foc_routine_align_b16 =
+{
+ .init = foc_routine_align_init_b16,
+ .deinit = foc_routine_align_deinit_b16,
+ .cfg = foc_routine_align_cfg_b16,
+ .run = foc_routine_align_run_b16,
+ .final = foc_routine_align_final_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_align_offset_run_b16
+ *
+ * Description:
+ * Run offset alignment routine
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+static int foc_align_offset_run_b16(FAR struct foc_align_b16_s *align,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Reset Align angle sensor */
+
+ if (align->cntr > align->cfg.offset_steps)
+ {
+ /* Align angle sensor */
+
+ if (align->cfg.cb.zero != NULL)
+ {
+ ret = align->cfg.cb.zero(align->cfg.cb.priv, in->angle);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: align offset callback failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+ /* Offset align done */
+
+ align->cntr = 0;
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Force IDLE mode */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+ }
+ else
+ {
+ /* Force DQ voltage vector */
+
+ out->dq_ref.q = align->cfg.volt;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = FOC_ALIGN_ANGLE;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+ }
+
+ /* Store offset if done */
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ align->final.offset = in->angle;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_move_b16
+ ****************************************************************************/
+
+static void foc_align_dir_move_b16(FAR struct foc_align_b16_s *align,
+ b16_t dir, b16_t dest)
+{
+ bool next = false;
+
+ DEBUGASSERT(align);
+
+ /* Update angle */
+
+ align->dir_angle += b16mulb16(dir, ALIGN_DIR_ANGLE_STEP);
+
+ /* Check destination */
+
+ if (dir == DIR_CW_B16)
+ {
+ if (align->dir_angle >= dest)
+ {
+ next = true;
+ }
+ }
+
+ else if (dir == DIR_CCW_B16)
+ {
+ if (align->dir_angle <= dest)
+ {
+ next = true;
+ }
+ }
+ else
+ {
+ DEBUGASSERT(0);
+ }
+
+ /* Next step */
+
+ if (next == true)
+ {
+ align->dir_step += 1;
+ }
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_hold_b16
+ ****************************************************************************/
+
+static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
+ b16_t dir, bool last, bool diff)
+{
+ DEBUGASSERT(align);
+
+ /* Lock angle */
+
+ align->dir_angle = align->dir_angle;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+
+ if (align->cntr > ALIGN_DIR_HOLD_CNTR)
+ {
+ /* Next step */
+
+ align->dir_step += 1;
+
+ /* Accumulate diff */
+
+ if (diff == true)
+ {
+ if (dir == DIR_CW_B16)
+ {
+ align->diff_cw += (align->angle_now - align->angle_last);
+ }
+ else if (dir == DIR_CCW_B16)
+ {
+ align->diff_ccw += (align->angle_now - align->angle_last);
+ }
+ else
+ {
+ DEBUGASSERT(0);
+ }
+ }
+
+ /* Store last angle */
+
+ if (last == true)
+ {
+ align->angle_last = align->angle_now;
+ }
+
+ /* Reset counter */
+
+ align->cntr = 0;
+ }
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_run_b16
+ *
+ * Description:
+ * Run direction alignment routine
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_align_dir_run_b16(FAR struct foc_align_b16_s *align,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ bool last = false;
+
+ /* Store angle now */
+
+ align->angle_now = in->angle;
+
+ /* Handle dir align */
+
+ if (align->dir_step == 0)
+ {
+ /* Start with position 0 */
+
+ align->dir_angle = ALIGN_DIR_ANGLE_HOLD_0;
+
+ /* Step 0 - hold position 0 */
+
+ foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
+ }
+
+ else if (align->dir_step == 1)
+ {
+ /* Step 1 - move motor in CW direction to position 1 */
+
+ foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_1);
+ }
+
+ else if (align->dir_step == 2)
+ {
+ /* Step 2 - hold position 1 */
+
+ foc_align_dir_hold_b16(align, DIR_CW_B16, true, false);
+ }
+
+ else if (align->dir_step == 3)
+ {
+ /* Step 3 - move motor in CW direction to position 2 */
+
+ foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_2);
+ }
+
+ else if (align->dir_step == 4)
+ {
+ /* Step 4 - hold position 2 */
+
+ foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
+ }
+
+ else if (align->dir_step == 5)
+ {
+ /* Step 5 - move motor in CW direction to position 3 */
+
+ foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_3);
+ }
+
+ else if (align->dir_step == 6)
+ {
+ /* Step 6 - hold position 3 */
+
+ foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
+ }
+
+ else if (align->dir_step == 6)
+ {
+ /* Step 6 - move motor in CW direction to position 4 */
+
+ foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_4);
+ }
+
+ else if (align->dir_step == 7)
+ {
+ /* Step 7 - hold position 4 */
+
+ foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
+ }
+
+ else if (align->dir_step == 8)
+ {
+ /* Step 8 - move motor in CCW direction to position 3 */
+
+ foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_3);
+ }
+
+ else if (align->dir_step == 9)
+ {
+ /* Step 9 - hold position 3 */
+
+ foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
+ }
+
+ else if (align->dir_step == 10)
+ {
+ /* Step 10 - move motor in CCW direction to position 2 */
+
+ foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_2);
+ }
+
+ else if (align->dir_step == 11)
+ {
+ /* Step 11 - hold position 2 */
+
+ foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
+ }
+
+ else if (align->dir_step == 12)
+ {
+ /* Step 12 - move motor in CCW direction to position 1 */
+
+ foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_1);
+ }
+
+ else if (align->dir_step == 13)
+ {
+ /* Step 13 - hold position 1 */
+
+ foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
+ }
+
+ else if (align->dir_step == 14)
+ {
+ /* Step 14 - move motor in CCW direction to position 0 */
+
+ foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_0);
+ }
+
+ else if (align->dir_step == 15)
+ {
+ /* Step 15 - hold position 0 */
+
+ foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
+ }
+
+ else
+ {
+ /* Set flag */
+
+ last = true;
+
+ /* Get sensor direction according to sampled data */
+
+ if (align->diff_cw > 0 && align->diff_ccw < 0)
+ {
+ align->final.dir = DIR_CW_B16;
+ }
+
+ else if (align->diff_cw < 0 && align->diff_ccw > 0)
+ {
+ align->final.dir = DIR_CCW_B16;
+ }
+
+ else
+ {
+ /* Invalid data */
+
+ FOCLIBERR("ERROR: direction align failed !\n");
+
+ align->final.dir = DIR_NONE_B16;
+ ret = -EINVAL;
+
+ goto errout;
+ }
+
+ /* Align angle sensor */
+
+ if (align->cfg.cb.dir != NULL)
+ {
+ ret = align->cfg.cb.dir(align->cfg.cb.priv, align->final.dir);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: align dir callback failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+ /* Direction alignment done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+ }
+
+ /* Force DQ voltage vector */
+
+ out->dq_ref.q = align->cfg.volt;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = align->dir_angle;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+errout:
+
+ /* Force current to zero if alignment done */
+
+ if (last == true)
+ {
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_align_idle_run_b16
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+static int foc_align_idle_run_b16(FAR struct foc_align_b16_s *align,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+
+ if (align->cntr > IDLE_STEPS)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ align->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_init_b16
+ *
+ * Description:
+ * Initialize the FOC align routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_align_init_b16(FAR foc_routine_b16_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_align_b16_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_deinit_b16
+ *
+ * Description:
+ * Deinitialize the FOC align routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_cfg_b16
+ *
+ * Description:
+ * Configure the FOC align routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to align routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
+{
+ FAR struct foc_align_b16_s *a = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(cfg);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&a->cfg, cfg, sizeof(struct foc_routine_align_cfg_b16_s));
+
+ /* Verify configuration */
+
+ if (a->cfg.volt <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (a->cfg.offset_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (a->cfg.cb.zero == NULL || a->cfg.cb.dir == NULL ||
+ a->cfg.cb.priv == NULL)
+ {
+ FOCLIBWARN("WARN: no align cb data!\n");
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_run_b16
+ *
+ * Description:
+ * Run the FOC align routine step (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ FAR struct foc_align_b16_s *a = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle alignment stage */
+
+ switch (a->stage)
+ {
+ case FOC_ALIGN_RUN_INIT:
+ {
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_OFFSET:
+ {
+ /* Align zero procedure */
+
+ ret = foc_align_offset_run_b16(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN OFFSET done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_DIR:
+ {
+ /* Align direction procedure */
+
+ ret = foc_align_dir_run_b16(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN DIR done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_IDLE:
+ {
+ /* De-energetize motor */
+
+ ret = foc_align_idle_run_b16(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN IDLE done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid alignment stage %d\n", a->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_final_b16
+ *
+ * Description:
+ * Finalize the FOC routine data (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC align routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
+{
+ FAR struct foc_align_b16_s *a = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &a->final, sizeof(struct foc_routine_aling_final_b16_s));
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_align.c b/industry/foc/float/foc_align.c
new file mode 100644
index 0000000..6c3c266
--- /dev/null
+++ b/industry/foc/float/foc_align.c
@@ -0,0 +1,825 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_align.c
+ * This file implements angle sensor alignment routine for float32
+ *
+ * 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 "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/float/foc_align.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Align to -90deg = 270deg */
+
+#define FOC_ALIGN_ANGLE (3*M_PI/2)
+
+/* Direction alignment */
+
+#define ALIGN_DIR_ANGLE_STEP (0.001f)
+#define ALIGN_DIR_ANGLE_HOLD_0 (0.0f)
+#define ALIGN_DIR_ANGLE_HOLD_1 (M_PI/3)
+#define ALIGN_DIR_ANGLE_HOLD_2 (2*M_PI/3)
+#define ALIGN_DIR_ANGLE_HOLD_3 (M_PI)
+#define ALIGN_DIR_ANGLE_HOLD_4 (4*M_PI/3)
+#define ALIGN_DIR_HOLD_CNTR (10)
+
+/* IDLE steps */
+
+#define IDLE_STEPS (500)
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Align stage */
+
+enum foc_align_run_stage_e
+{
+ FOC_ALIGN_RUN_INIT,
+ FOC_ALIGN_RUN_OFFSET,
+ FOC_ALIGN_RUN_DIR,
+ FOC_ALIGN_RUN_IDLE,
+ FOC_ALIGN_RUN_DONE
+};
+
+/* Align routine private data */
+
+struct foc_align_f32_s
+{
+ struct foc_routine_align_cfg_f32_s cfg;
+ struct foc_routine_aling_final_f32_s final;
+ int cntr;
+ int stage;
+ int dir_step;
+ float dir_angle;
+ float angle_last;
+ float angle_now;
+ float diff_cw;
+ float diff_ccw;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_align_init_f32(FAR foc_routine_f32_t *r);
+void foc_routine_align_deinit_f32(FAR foc_routine_f32_t *r);
+int foc_routine_align_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg);
+int foc_routine_align_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out);
+int foc_routine_align_final_f32(FAR foc_routine_f32_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_f32_s g_foc_routine_align_f32 =
+{
+ .init = foc_routine_align_init_f32,
+ .deinit = foc_routine_align_deinit_f32,
+ .cfg = foc_routine_align_cfg_f32,
+ .run = foc_routine_align_run_f32,
+ .final = foc_routine_align_final_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_align_offset_run_f32
+ *
+ * Description:
+ * Run offset alignment routine
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+static int foc_align_offset_run_f32(FAR struct foc_align_f32_s *align,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Reset Align angle sensor */
+
+ if (align->cntr > align->cfg.offset_steps)
+ {
+ /* Align angle sensor */
+
+ if (align->cfg.cb.zero != NULL)
+ {
+ ret = align->cfg.cb.zero(align->cfg.cb.priv, in->angle);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: align offset callback failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+ /* Offset align done */
+
+ align->cntr = 0;
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Force IDLE mode */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+ }
+ else
+ {
+ /* Force DQ voltage vector */
+
+ out->dq_ref.q = align->cfg.volt;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = FOC_ALIGN_ANGLE;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+ }
+
+ /* Store offset if done */
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ align->final.offset = in->angle;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_move_f32
+ ****************************************************************************/
+
+static void foc_align_dir_move_f32(FAR struct foc_align_f32_s *align,
+ float dir, float dest)
+{
+ bool next = false;
+
+ DEBUGASSERT(align);
+
+ /* Update angle */
+
+ align->dir_angle += dir * ALIGN_DIR_ANGLE_STEP;
+
+ /* Check destination */
+
+ if (dir == DIR_CW)
+ {
+ if (align->dir_angle >= dest)
+ {
+ next = true;
+ }
+ }
+
+ else if (dir == DIR_CCW)
+ {
+ if (align->dir_angle <= dest)
+ {
+ next = true;
+ }
+ }
+ else
+ {
+ DEBUGASSERT(0);
+ }
+
+ /* Next step */
+
+ if (next == true)
+ {
+ align->dir_step += 1;
+ }
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_hold_f32
+ ****************************************************************************/
+
+static void foc_align_dir_hold_f32(FAR struct foc_align_f32_s *align,
+ float dir, bool last, bool diff)
+{
+ DEBUGASSERT(align);
+
+ /* Lock angle */
+
+ align->dir_angle = align->dir_angle;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+
+ if (align->cntr > ALIGN_DIR_HOLD_CNTR)
+ {
+ /* Next step */
+
+ align->dir_step += 1;
+
+ /* Accumulate diff */
+
+ if (diff == true)
+ {
+ if (dir == DIR_CW)
+ {
+ align->diff_cw += (align->angle_now - align->angle_last);
+ }
+ else if (dir == DIR_CCW)
+ {
+ align->diff_ccw += (align->angle_now - align->angle_last);
+ }
+ else
+ {
+ DEBUGASSERT(0);
+ }
+ }
+
+ /* Store last angle */
+
+ if (last == true)
+ {
+ align->angle_last = align->angle_now;
+ }
+
+ /* Reset counter */
+
+ align->cntr = 0;
+ }
+}
+
+/****************************************************************************
+ * Name: foc_align_dir_run_f32
+ *
+ * Description:
+ * Run direction alignment routine
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_align_dir_run_f32(FAR struct foc_align_f32_s *align,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ bool last = false;
+
+ /* Store angle now */
+
+ align->angle_now = in->angle;
+
+ /* Handle dir align */
+
+ if (align->dir_step == 0)
+ {
+ /* Start with position 0 */
+
+ align->dir_angle = ALIGN_DIR_ANGLE_HOLD_0;
+
+ /* Step 0 - hold position 0 */
+
+ foc_align_dir_hold_f32(align, DIR_CW, true, true);
+ }
+
+ else if (align->dir_step == 1)
+ {
+ /* Step 1 - move motor in CW direction to position 1 */
+
+ foc_align_dir_move_f32(align, DIR_CW, ALIGN_DIR_ANGLE_HOLD_1);
+ }
+
+ else if (align->dir_step == 2)
+ {
+ /* Step 2 - hold position 1 */
+
+ foc_align_dir_hold_f32(align, DIR_CW, true, false);
+ }
+
+ else if (align->dir_step == 3)
+ {
+ /* Step 3 - move motor in CW direction to position 2 */
+
+ foc_align_dir_move_f32(align, DIR_CW, ALIGN_DIR_ANGLE_HOLD_2);
+ }
+
+ else if (align->dir_step == 4)
+ {
+ /* Step 4 - hold position 2 */
+
+ foc_align_dir_hold_f32(align, DIR_CW, true, true);
+ }
+
+ else if (align->dir_step == 5)
+ {
+ /* Step 5 - move motor in CW direction to position 3 */
+
+ foc_align_dir_move_f32(align, DIR_CW, ALIGN_DIR_ANGLE_HOLD_3);
+ }
+
+ else if (align->dir_step == 6)
+ {
+ /* Step 6 - hold position 3 */
+
+ foc_align_dir_hold_f32(align, DIR_CW, true, true);
+ }
+
+ else if (align->dir_step == 6)
+ {
+ /* Step 6 - move motor in CW direction to position 4 */
+
+ foc_align_dir_move_f32(align, DIR_CW, ALIGN_DIR_ANGLE_HOLD_4);
+ }
+
+ else if (align->dir_step == 7)
+ {
+ /* Step 7 - hold position 4 */
+
+ foc_align_dir_hold_f32(align, DIR_CW, true, true);
+ }
+
+ else if (align->dir_step == 8)
+ {
+ /* Step 8 - move motor in CCW direction to position 3 */
+
+ foc_align_dir_move_f32(align, DIR_CCW, ALIGN_DIR_ANGLE_HOLD_3);
+ }
+
+ else if (align->dir_step == 9)
+ {
+ /* Step 9 - hold position 3 */
+
+ foc_align_dir_hold_f32(align, DIR_CCW, true, true);
+ }
+
+ else if (align->dir_step == 10)
+ {
+ /* Step 10 - move motor in CCW direction to position 2 */
+
+ foc_align_dir_move_f32(align, DIR_CCW, ALIGN_DIR_ANGLE_HOLD_2);
+ }
+
+ else if (align->dir_step == 11)
+ {
+ /* Step 11 - hold position 2 */
+
+ foc_align_dir_hold_f32(align, DIR_CCW, true, true);
+ }
+
+ else if (align->dir_step == 12)
+ {
+ /* Step 12 - move motor in CCW direction to position 1 */
+
+ foc_align_dir_move_f32(align, DIR_CCW, ALIGN_DIR_ANGLE_HOLD_1);
+ }
+
+ else if (align->dir_step == 13)
+ {
+ /* Step 13 - hold position 1 */
+
+ foc_align_dir_hold_f32(align, DIR_CCW, true, true);
+ }
+
+ else if (align->dir_step == 14)
+ {
+ /* Step 14 - move motor in CCW direction to position 0 */
+
+ foc_align_dir_move_f32(align, DIR_CCW, ALIGN_DIR_ANGLE_HOLD_0);
+ }
+
+ else if (align->dir_step == 15)
+ {
+ /* Step 15 - hold position 0 */
+
+ foc_align_dir_hold_f32(align, DIR_CCW, true, true);
+ }
+
+ else
+ {
+ /* Set flag */
+
+ last = true;
+
+ /* Get sensor direction according to sampled data */
+
+ if (align->diff_cw > 0.0f && align->diff_ccw < 0.0f)
+ {
+ align->final.dir = DIR_CW;
+ }
+
+ else if (align->diff_cw < 0.0f && align->diff_ccw > 0.0f)
+ {
+ align->final.dir = DIR_CCW;
+ }
+
+ else
+ {
+ /* Invalid data */
+
+ FOCLIBERR("ERROR: direction align failed !\n");
+
+ align->final.dir = DIR_NONE;
+ ret = -EINVAL;
+
+ goto errout;
+ }
+
+ /* Align angle sensor */
+
+ if (align->cfg.cb.dir != NULL)
+ {
+ ret = align->cfg.cb.dir(align->cfg.cb.priv, align->final.dir);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: align dir callback failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+ /* Direction alignment done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+ }
+
+ /* Force DQ voltage vector */
+
+ out->dq_ref.q = align->cfg.volt;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = align->dir_angle;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+errout:
+
+ /* Force current to zero if alignment done */
+
+ if (last == true)
+ {
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_align_idle_run_f32
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * align - pointer to FOC align routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+static int foc_align_idle_run_f32(FAR struct foc_align_f32_s *align,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+
+ if (align->cntr > IDLE_STEPS)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ align->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_init_f32
+ *
+ * Description:
+ * Initialize the FOC align routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_align_init_f32(FAR foc_routine_f32_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_align_f32_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_deinit_f32
+ *
+ * Description:
+ * Deinitialize the FOC align routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_align_deinit_f32(FAR foc_routine_f32_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_cfg_f32
+ *
+ * Description:
+ * Configure the FOC align routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to align routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
+{
+ FAR struct foc_align_f32_s *a = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(cfg);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&a->cfg, cfg, sizeof(struct foc_routine_align_cfg_f32_s));
+
+ /* Verify configuration */
+
+ if (a->cfg.volt <= 0.0f)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (a->cfg.offset_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (a->cfg.cb.zero == NULL || a->cfg.cb.dir == NULL ||
+ a->cfg.cb.priv == NULL)
+ {
+ FOCLIBWARN("WARN: no align cb data!\n");
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_run_f32
+ *
+ * Description:
+ * Run the FOC align routine step (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ FAR struct foc_align_f32_s *a = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle alignment stage */
+
+ switch (a->stage)
+ {
+ case FOC_ALIGN_RUN_INIT:
+ {
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_OFFSET:
+ {
+ /* Align zero procedure */
+
+ ret = foc_align_offset_run_f32(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN OFFSET done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_DIR:
+ {
+ /* Align direction procedure */
+
+ ret = foc_align_dir_run_f32(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN DIR done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_IDLE:
+ {
+ /* De-energetize motor */
+
+ ret = foc_align_idle_run_f32(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN IDLE done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_ALIGN_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid alignment stage %d\n", a->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_align_final_f32
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC align routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_align_final_f32(FAR foc_routine_f32_t *r, FAR void *data)
+{
+ FAR struct foc_align_f32_s *a = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get aling data */
+
+ DEBUGASSERT(r->data);
+ a = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &a->final, sizeof(struct foc_routine_aling_final_f32_s));
+
+ return OK;
+}
[incubator-nuttx-apps] 01/03: industry/foc: add general FOC routine
handler
Posted by xi...@apache.org.
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 084cfa4e5e3dac43809436e78e3630260f3f5c89
Author: raiden00pl <ra...@railab.me>
AuthorDate: Fri Oct 29 13:10:39 2021 +0200
industry/foc: add general FOC routine handler
---
include/industry/foc/fixed16/foc_routine.h | 140 ++++++++++++++++++++++
include/industry/foc/float/foc_routine.h | 140 ++++++++++++++++++++++
industry/foc/Makefile | 2 +
industry/foc/fixed16/foc_routine.c | 179 +++++++++++++++++++++++++++++
industry/foc/float/foc_routine.c | 179 +++++++++++++++++++++++++++++
5 files changed, 640 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_routine.h b/include/industry/foc/fixed16/foc_routine.h
new file mode 100644
index 0000000..409b334
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_routine.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_routine.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FIXED16_FOC_ROUTINE_H
+#define __INDUSTRY_FOC_FIXED16_FOC_ROUTINE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include "industry/foc/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Routine run return */
+
+enum foc_routine_run_e
+{
+ FOC_ROUTINE_RUN_NOTDONE = 0,
+ FOC_ROUTINE_RUN_DONE = 1
+};
+
+/* FOC routine input */
+
+struct foc_routine_in_b16_s
+{
+ FAR struct foc_state_b16_s *foc_state; /* FOC controller state */
+ b16_t angle; /* Angle now */
+ b16_t vel; /* Velocity now */
+ b16_t vbus; /* VBUS now */
+};
+
+/* FOC routine output */
+
+struct foc_routine_out_b16_s
+{
+ dq_frame_b16_t dq_ref; /* Output DQ reference */
+ dq_frame_b16_t vdq_comp; /* Output DQ voltage compensation */
+ b16_t angle; /* Output phase angle */
+ int foc_mode; /* Output FOC mode */
+};
+
+/* Forward declaration */
+
+typedef struct foc_routine_b16_s foc_routine_b16_t;
+
+/* FOC routine operations */
+
+struct foc_routine_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_routine_b16_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_routine_b16_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_routine_b16_t *h, FAR void *cfg);
+
+ /* Run routine */
+
+ CODE int (*run)(FAR foc_routine_b16_t *h,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out);
+
+ /* Run routine */
+
+ CODE int (*final)(FAR foc_routine_b16_t *h, FAR void *data);
+};
+
+/* FOC routine data */
+
+struct foc_routine_b16_s
+{
+ FAR struct foc_routine_ops_b16_s *ops;
+ FAR void *data;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_routine_init_b16
+ ****************************************************************************/
+
+int foc_routine_init_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_ops_b16_s *ops);
+
+/****************************************************************************
+ * Name: foc_routine_deinit_b16
+ ****************************************************************************/
+
+int foc_routine_deinit_b16(FAR foc_routine_b16_t *r);
+
+/****************************************************************************
+ * Name: foc_routine_cfg_b16
+ ****************************************************************************/
+
+int foc_routine_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_routine_run_b16
+ ****************************************************************************/
+
+int foc_routine_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out);
+
+/****************************************************************************
+ * Name: foc_routine_final_b16
+ ****************************************************************************/
+
+int foc_routine_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_ROUTINE_H */
diff --git a/include/industry/foc/float/foc_routine.h b/include/industry/foc/float/foc_routine.h
new file mode 100644
index 0000000..892e5e0
--- /dev/null
+++ b/include/industry/foc/float/foc_routine.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_routine.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FLOAT_FOC_ROUTINE_H
+#define __INDUSTRY_FOC_FLOAT_FOC_ROUTINE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include "industry/foc/float/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Routine run return */
+
+enum foc_routine_run_e
+{
+ FOC_ROUTINE_RUN_NOTDONE = 0,
+ FOC_ROUTINE_RUN_DONE = 1
+};
+
+/* FOC routine input */
+
+struct foc_routine_in_f32_s
+{
+ FAR struct foc_state_f32_s *foc_state; /* FOC controller state */
+ float angle; /* Angle now */
+ float vel; /* Velocity now */
+ float vbus; /* VBUS now */
+};
+
+/* FOC routine output */
+
+struct foc_routine_out_f32_s
+{
+ dq_frame_f32_t dq_ref; /* Output DQ reference */
+ dq_frame_f32_t vdq_comp; /* Output DQ voltage compensation */
+ float angle; /* Output phase angle */
+ int foc_mode; /* Output FOC mode */
+};
+
+/* Forward declaration */
+
+typedef struct foc_routine_f32_s foc_routine_f32_t;
+
+/* FOC routine operations */
+
+struct foc_routine_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_routine_f32_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_routine_f32_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_routine_f32_t *h, FAR void *cfg);
+
+ /* Run routine */
+
+ CODE int (*run)(FAR foc_routine_f32_t *h,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out);
+
+ /* Run routine */
+
+ CODE int (*final)(FAR foc_routine_f32_t *h, FAR void *data);
+};
+
+/* FOC routine data */
+
+struct foc_routine_f32_s
+{
+ FAR struct foc_routine_ops_f32_s *ops;
+ FAR void *data;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_routine_init_f32
+ ****************************************************************************/
+
+int foc_routine_init_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_ops_f32_s *ops);
+
+/****************************************************************************
+ * Name: foc_routine_deinit_f32
+ ****************************************************************************/
+
+int foc_routine_deinit_f32(FAR foc_routine_f32_t *r);
+
+/****************************************************************************
+ * Name: foc_routine_cfg_f32
+ ****************************************************************************/
+
+int foc_routine_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_routine_run_f32
+ ****************************************************************************/
+
+int foc_routine_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out);
+
+/****************************************************************************
+ * Name: foc_routine_final_f32
+ ****************************************************************************/
+
+int foc_routine_final_f32(FAR foc_routine_f32_t *r, FAR void *data);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_ROUTINE_H */
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 29b66bd..60f00ca 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -32,6 +32,7 @@ CSRCS += float/foc_ramp.c
CSRCS += float/foc_handler.c
CSRCS += float/foc_angle.c
CSRCS += float/foc_velocity.c
+CSRCS += float/foc_routine.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += float/foc_openloop.c
endif
@@ -61,6 +62,7 @@ CSRCS += fixed16/foc_ramp.c
CSRCS += fixed16/foc_handler.c
CSRCS += fixed16/foc_angle.c
CSRCS += fixed16/foc_velocity.c
+CSRCS += fixed16/foc_routine.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += fixed16/foc_openloop.c
endif
diff --git a/industry/foc/fixed16/foc_routine.c b/industry/foc/fixed16/foc_routine.c
new file mode 100644
index 0000000..bba1e61
--- /dev/null
+++ b/industry/foc/fixed16/foc_routine.c
@@ -0,0 +1,179 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_routine.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 "industry/foc/foc_log.h"
+#include "industry/foc/fixed16/foc_routine.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_routine_init_b16
+ *
+ * Description:
+ * Initialize the FOC routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * ops - pointer to FOC routine operations
+ *
+ ****************************************************************************/
+
+int foc_routine_init_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_ops_b16_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(ops);
+
+ /* Routine ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset routine */
+
+ memset(r, 0, sizeof(foc_routine_b16_t));
+
+ /* Connect ops */
+
+ r->ops = ops;
+
+ /* Initialize routine */
+
+ ret = r->ops->init(r);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_deinit_b16
+ *
+ * Description:
+ * De-initialize the FOC routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_deinit_b16(FAR foc_routine_b16_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Deinitialize routine */
+
+ r->ops->deinit(r);
+
+ /* Reset data */
+
+ memset(r, 0, sizeof(foc_routine_b16_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_cfg_b16
+ *
+ * Description:
+ * Configure the FOC routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(cfg);
+
+ return r->ops->cfg(r, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_routine_run_b16
+ *
+ * Description:
+ * Run the FOC routine (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run routine handler */
+
+ return r->ops->run(r, in, out);
+}
+
+/****************************************************************************
+ * Name: foc_routine_final_b16
+ *
+ * Description:
+ * Finalize the FOC routine data (fixed16)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Finalize routine */
+
+ return r->ops->final(r, data);
+}
diff --git a/industry/foc/float/foc_routine.c b/industry/foc/float/foc_routine.c
new file mode 100644
index 0000000..767fb18
--- /dev/null
+++ b/industry/foc/float/foc_routine.c
@@ -0,0 +1,179 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_routine.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 "industry/foc/foc_log.h"
+#include "industry/foc/float/foc_routine.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_routine_init_f32
+ *
+ * Description:
+ * Initialize the FOC routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * ops - pointer to FOC routine operations
+ *
+ ****************************************************************************/
+
+int foc_routine_init_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_ops_f32_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(ops);
+
+ /* Routine ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset routine */
+
+ memset(r, 0, sizeof(foc_routine_f32_t));
+
+ /* Connect ops */
+
+ r->ops = ops;
+
+ /* Initialize routine */
+
+ ret = r->ops->init(r);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_deinit_f32
+ *
+ * Description:
+ * De-initialize the FOC routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_deinit_f32(FAR foc_routine_f32_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Deinitialize routine */
+
+ r->ops->deinit(r);
+
+ /* Reset data */
+
+ memset(r, 0, sizeof(foc_routine_f32_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_cfg_f32
+ *
+ * Description:
+ * Configure the FOC routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(cfg);
+
+ return r->ops->cfg(r, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_routine_run_f32
+ *
+ * Description:
+ * Run the FOC routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run routine handler */
+
+ return r->ops->run(r, in, out);
+}
+
+/****************************************************************************
+ * Name: foc_routine_final_f32
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_final_f32(FAR foc_routine_f32_t *r, FAR void *data)
+{
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Finalize routine */
+
+ return r->ops->final(r, data);
+}
[incubator-nuttx-apps] 03/03: industry/foc: add motor
identification routine
Posted by xi...@apache.org.
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 f009d68a914da4d9981ca21527e10db33abf0339
Author: raiden00pl <ra...@railab.me>
AuthorDate: Fri Oct 29 13:35:48 2021 +0200
industry/foc: add motor identification routine
---
include/industry/foc/fixed16/foc_ident.h | 65 ++++
include/industry/foc/float/foc_ident.h | 65 ++++
industry/foc/Kconfig | 6 +
industry/foc/Makefile | 6 +
industry/foc/fixed16/foc_ident.c | 608 +++++++++++++++++++++++++++++++
industry/foc/float/foc_ident.c | 601 ++++++++++++++++++++++++++++++
6 files changed, 1351 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_ident.h b/include/industry/foc/fixed16/foc_ident.h
new file mode 100644
index 0000000..276fa63
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_ident.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_ident.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FIXED16_FOC_IDENT_H
+#define __INDUSTRY_FOC_FIXED16_FOC_IDENT_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#include "industry/foc/fixed16/foc_routine.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Identification routine configuration */
+
+struct foc_routine_ident_cfg_b16_s
+{
+ b16_t per; /* Routine period in sec */
+ b16_t res_current; /* Resistance measurement current */
+ b16_t ind_volt; /* Inductance measurement current */
+ int res_steps; /* Resistance measurement steps */
+ int ind_steps; /* Inductance measurement steps */
+ int idle_steps; /* IDLE steps */
+};
+
+/* Identification routine final data */
+
+struct foc_routine_ident_final_b16_s
+{
+ bool ready; /* Result ready */
+ b16_t res; /* Phase resistance */
+ b16_t ind; /* Phase inductance */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_b16_s g_foc_routine_ident_b16;
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_IDENT_H */
diff --git a/include/industry/foc/float/foc_ident.h b/include/industry/foc/float/foc_ident.h
new file mode 100644
index 0000000..d7172f8
--- /dev/null
+++ b/include/industry/foc/float/foc_ident.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_ident.h
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FLOAT_FOC_IDENT_H
+#define __INDUSTRY_FOC_FLOAT_FOC_IDENT_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#include "industry/foc/float/foc_routine.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Identification routine configuration */
+
+struct foc_routine_ident_cfg_f32_s
+{
+ float per; /* Routine period in sec */
+ float res_current; /* Resistance measurement current */
+ float ind_volt; /* Inductance measurement current */
+ int res_steps; /* Resistance measurement steps */
+ int ind_steps; /* Inductance measurement steps */
+ int idle_steps; /* IDLE steps */
+};
+
+/* Identification routine final data */
+
+struct foc_routine_ident_final_f32_s
+{
+ bool ready; /* Result ready */
+ float res; /* Phase resistance */
+ float ind; /* Phase inductance */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_f32_s g_foc_routine_ident_f32;
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_IDENT_H */
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index 5124cea..aff1209 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -87,4 +87,10 @@ config INDUSTRY_FOC_ALIGN
---help---
Enable support for angle sensor alignment routine (zero offset + direction)
+config INDUSTRY_FOC_IDENT
+ bool "FOC motor identification routine"
+ default n
+ ---help---
+ Enable support for motor identification routine (phase resistance and phase inductance)
+
endif
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 83489f8..35583ac 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -54,6 +54,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
CSRCS += float/foc_align.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
+CSRCS += float/foc_ident.c
+endif
endif
@@ -87,6 +90,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
CSRCS += fixed16/foc_align.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
+CSRCS += fixed16/foc_ident.c
+endif
endif
diff --git a/industry/foc/fixed16/foc_ident.c b/industry/foc/fixed16/foc_ident.c
new file mode 100644
index 0000000..65dab35
--- /dev/null
+++ b/industry/foc/fixed16/foc_ident.c
@@ -0,0 +1,608 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_ident.c
+ * This file implements motor ident routine for fixed16
+ *
+ * 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 "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/fixed16/foc_ident.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define IDENT_PI_KP (ftob16(0.0f))
+#define IDENT_PI_KI (ftob16(0.05f))
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Identification stages */
+
+enum foc_ident_run_stage_e
+{
+ FOC_IDENT_RUN_INIT,
+ FOC_IDENT_RUN_IDLE1,
+ FOC_IDENT_RUN_RES,
+ FOC_IDENT_RUN_IDLE2,
+ FOC_IDENT_RUN_IND,
+ FOC_IDENT_RUN_IDLE3,
+ FOC_IDENT_RUN_DONE
+};
+
+/* Ident routine private data */
+
+struct foc_ident_b16_s
+{
+ struct foc_routine_ident_cfg_b16_s cfg; /* Ident configuration */
+ struct foc_routine_ident_final_b16_s final; /* Ident final result */
+ pid_controller_b16_t pi; /* PI controller for res */
+ int cntr; /* Helper counter */
+ int stage; /* Ident stage */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r);
+void foc_routine_ident_deinit_b16(FAR foc_routine_b16_t *r);
+int foc_routine_ident_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
+int foc_routine_ident_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out);
+int foc_routine_ident_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_b16_s g_foc_routine_ident_b16 =
+{
+ .init = foc_routine_ident_init_b16,
+ .deinit = foc_routine_ident_deinit_b16,
+ .cfg = foc_routine_ident_cfg_b16,
+ .run = foc_routine_ident_run_b16,
+ .final = foc_routine_ident_final_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ident_idle_run_b16
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_idle_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.idle_steps)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_res_run_b16
+ *
+ * Description:
+ * Run resistance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_res_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ b16_t err = 0;
+ b16_t vref = 0;
+
+ /* Initialize PI controller */
+
+ if (ident->cntr == 0)
+ {
+ pi_controller_init_b16(&ident->pi, IDENT_PI_KP, IDENT_PI_KI);
+ }
+
+ /* PI saturation */
+
+ pi_saturation_set_b16(&ident->pi, 0, in->vbus);
+
+ /* Lock motor with given current */
+
+ err = ident->cfg.res_current - in->foc_state->idq.d;
+ vref = pi_controller_b16(&ident->pi, err);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.res_steps)
+ {
+ /* Get resistance */
+
+ ident->final.res = b16divb16(vref, ident->cfg.res_current);
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Resistance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_ind_run_b16
+ *
+ * Description:
+ * Run inductance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ b16_t vref = 0;
+ b16_t curr1_avg = 0;
+ b16_t curr2_avg = 0;
+ b16_t delta_curr = 0;
+ static b16_t sign = b16ONE;
+ static b16_t curr1_sum = 0;
+ static b16_t curr2_sum = 0;
+ b16_t tmp1 = 0;
+ b16_t tmp2 = 0;
+ b16_t tmp3 = 0;
+
+ /* If previous sign was -1 then we have top current,
+ * if previous sing was +1 then we have bottom current.
+ */
+
+ if (sign > 0)
+ {
+ /* Average bottm current */
+
+ curr1_sum += in->foc_state->idq.d;
+ }
+ else
+ {
+ /* Average top current */
+
+ curr2_sum += in->foc_state->idq.d;
+ }
+
+ /* Invert voltage to generate square wave D voltage */
+
+ sign = -sign;
+ vref = b16mulb16(sign, ident->cfg.ind_volt);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.ind_steps)
+ {
+ /* Half samples from curr1, other half from curr2 */
+
+ tmp1 = b16muli(curr1_sum, 2);
+ tmp2 = b16muli(curr2_sum, 2);
+
+ curr1_avg = b16divb16(tmp1, ident->cntr);
+ curr2_avg = b16divb16(tmp2, ident->cntr);
+
+ /* Average delta current */
+
+ delta_curr = curr1_avg - curr2_avg;
+
+ /* Get inductance
+ * L = V * t / dI
+ *
+ * where:
+ * t = per
+ * V = ind_volt
+ * dI = avg(curr_bottom) - avg(curr_top)
+ */
+
+ tmp3 = b16mulb16(ident->cfg.ind_volt, ident->cfg.per);
+ ident->final.ind = b16divb16(tmp3, delta_curr);
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Inductance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+
+ /* Reset static data */
+
+ sign = b16ONE;
+ curr1_sum = 0;
+ curr2_sum = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_init_b16
+ *
+ * Description:
+ * Initialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_ident_b16_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_deinit_b16
+ *
+ * Description:
+ * Deinitialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_ident_deinit_b16(FAR foc_routine_b16_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_cfg_b16
+ *
+ * Description:
+ * Configure the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to ident routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&i->cfg, cfg, sizeof(struct foc_routine_ident_cfg_b16_s));
+
+ /* Verify configuration */
+
+ if (i->cfg.per <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_current <= 0 || i->cfg.ind_volt <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
+ i->cfg.idle_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_run_b16
+ *
+ * Description:
+ * Run the FOC ident routine step (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle ident stage */
+
+ switch (i->stage)
+ {
+ case FOC_IDENT_RUN_INIT:
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IDLE1:
+ case FOC_IDENT_RUN_IDLE2:
+ case FOC_IDENT_RUN_IDLE3:
+ {
+ /* De-energetize motor */
+
+ ret = foc_ident_idle_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_RES:
+ {
+ /* Resistance */
+
+ ret = foc_ident_res_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT RES done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IND:
+ {
+ /* Inductance */
+
+ ret = foc_ident_ind_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT IND done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid ident stage %d\n", i->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_final_b16
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC ident routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &i->final, sizeof(struct foc_routine_ident_final_b16_s));
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_ident.c b/industry/foc/float/foc_ident.c
new file mode 100644
index 0000000..f0a6117
--- /dev/null
+++ b/industry/foc/float/foc_ident.c
@@ -0,0 +1,601 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_ident.c
+ * This file implements motor ident routine for float32
+ *
+ * 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 "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/float/foc_ident.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define IDENT_PI_KP (0.0f)
+#define IDENT_PI_KI (0.05f)
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Identification stages */
+
+enum foc_ident_run_stage_e
+{
+ FOC_IDENT_RUN_INIT,
+ FOC_IDENT_RUN_IDLE1,
+ FOC_IDENT_RUN_RES,
+ FOC_IDENT_RUN_IDLE2,
+ FOC_IDENT_RUN_IND,
+ FOC_IDENT_RUN_IDLE3,
+ FOC_IDENT_RUN_DONE
+};
+
+/* Ident routine private data */
+
+struct foc_ident_f32_s
+{
+ struct foc_routine_ident_cfg_f32_s cfg; /* Ident configuration */
+ struct foc_routine_ident_final_f32_s final; /* Ident final result */
+ pid_controller_f32_t pi; /* PI controller for res */
+ int cntr; /* Helper counter */
+ int stage; /* Ident stage */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r);
+void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r);
+int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg);
+int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out);
+int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_f32_s g_foc_routine_ident_f32 =
+{
+ .init = foc_routine_ident_init_f32,
+ .deinit = foc_routine_ident_deinit_f32,
+ .cfg = foc_routine_ident_cfg_f32,
+ .run = foc_routine_ident_run_f32,
+ .final = foc_routine_ident_final_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ident_idle_run_f32
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_idle_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.idle_steps)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_res_run_f32
+ *
+ * Description:
+ * Run resistance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ float err = 0.0f;
+ float vref = 0.0f;
+
+ /* Initialize PI controller */
+
+ if (ident->cntr == 0)
+ {
+ pi_controller_init(&ident->pi, IDENT_PI_KP, IDENT_PI_KI);
+ }
+
+ /* PI saturation */
+
+ pi_saturation_set(&ident->pi, 0.0f, in->vbus);
+
+ /* Lock motor with given current */
+
+ err = ident->cfg.res_current - in->foc_state->idq.d;
+ vref = pi_controller(&ident->pi, err);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.res_steps)
+ {
+ /* Get resistance */
+
+ ident->final.res = vref / ident->cfg.res_current;
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Resistance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_ind_run_f32
+ *
+ * Description:
+ * Run inductance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ float vref = 0.0f;
+ float curr1_avg = 0.0f;
+ float curr2_avg = 0.0f;
+ float delta_curr = 0.0f;
+ static float sign = 1.0f;
+ static float curr1_sum = 0.0f;
+ static float curr2_sum = 0.0f;
+
+ /* If previous sign was -1 then we have top current,
+ * if previous sing was +1 then we have bottom current.
+ */
+
+ if (sign > 0)
+ {
+ /* Average bottm current */
+
+ curr1_sum += in->foc_state->idq.d;
+ }
+ else
+ {
+ /* Average top current */
+
+ curr2_sum += in->foc_state->idq.d;
+ }
+
+ /* Invert voltage to generate square wave D voltage */
+
+ sign = -sign;
+ vref = sign * ident->cfg.ind_volt;
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.ind_steps)
+ {
+ /* Half samples from curr1, other half from curr2 */
+
+ curr1_avg = 2 * curr1_sum / ident->cntr;
+ curr2_avg = 2 * curr2_sum / ident->cntr;
+
+ /* Average delta current */
+
+ delta_curr = curr1_avg - curr2_avg;
+
+ /* Get inductance
+ * L = V * t / dI
+ *
+ * where:
+ * t = per
+ * V = ind_volt
+ * dI = avg(curr_bottom) - avg(curr_top)
+ */
+
+ ident->final.ind = ident->cfg.ind_volt * ident->cfg.per / delta_curr;
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Inductance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+
+ /* Reset static data */
+
+ sign = 1.0f;
+ curr1_sum = 0.0f;
+ curr2_sum = 0.0f;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_init_f32
+ *
+ * Description:
+ * Initialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_ident_f32_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_deinit_f32
+ *
+ * Description:
+ * Deinitialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_cfg_f32
+ *
+ * Description:
+ * Configure the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to ident routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&i->cfg, cfg, sizeof(struct foc_routine_ident_cfg_f32_s));
+
+ /* Verify configuration */
+
+ if (i->cfg.per <= 0.0f)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
+ i->cfg.idle_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_run_f32
+ *
+ * Description:
+ * Run the FOC ident routine step (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle ident stage */
+
+ switch (i->stage)
+ {
+ case FOC_IDENT_RUN_INIT:
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IDLE1:
+ case FOC_IDENT_RUN_IDLE2:
+ case FOC_IDENT_RUN_IDLE3:
+ {
+ /* De-energetize motor */
+
+ ret = foc_ident_idle_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_RES:
+ {
+ /* Resistance */
+
+ ret = foc_ident_res_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT RES done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IND:
+ {
+ /* Inductance */
+
+ ret = foc_ident_ind_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT IND done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid ident stage %d\n", i->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_final_f32
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC ident routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &i->final, sizeof(struct foc_routine_ident_final_f32_s));
+
+ return OK;
+}