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/11/07 09:46:21 UTC
[incubator-nuttx-apps] 01/02: industry/foc/foc_align: add support
for sensor index search
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 54c0b9a9d87ecca0ba95c816b19a300242d8f932
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Nov 6 18:59:01 2021 +0100
industry/foc/foc_align: add support for sensor index search
---
include/industry/foc/fixed16/foc_routine.h | 3 +-
include/industry/foc/float/foc_routine.h | 3 +-
industry/foc/Kconfig | 10 ++
industry/foc/fixed16/foc_align.c | 178 +++++++++++++++++++++++++++++
industry/foc/float/foc_align.c | 178 +++++++++++++++++++++++++++++
5 files changed, 370 insertions(+), 2 deletions(-)
diff --git a/include/industry/foc/fixed16/foc_routine.h b/include/industry/foc/fixed16/foc_routine.h
index 409b334..320d8a8 100644
--- a/include/industry/foc/fixed16/foc_routine.h
+++ b/include/industry/foc/fixed16/foc_routine.h
@@ -46,7 +46,8 @@ enum foc_routine_run_e
struct foc_routine_in_b16_s
{
FAR struct foc_state_b16_s *foc_state; /* FOC controller state */
- b16_t angle; /* Angle now */
+ b16_t angle; /* Angle electrical now */
+ b16_t angle_m; /* Angle mechanical now */
b16_t vel; /* Velocity now */
b16_t vbus; /* VBUS now */
};
diff --git a/include/industry/foc/float/foc_routine.h b/include/industry/foc/float/foc_routine.h
index 892e5e0..fc3d686 100644
--- a/include/industry/foc/float/foc_routine.h
+++ b/include/industry/foc/float/foc_routine.h
@@ -46,7 +46,8 @@ enum foc_routine_run_e
struct foc_routine_in_f32_s
{
FAR struct foc_state_f32_s *foc_state; /* FOC controller state */
- float angle; /* Angle now */
+ float angle; /* Angle electrical now */
+ float angle_m; /* Angle mechanical now */
float vel; /* Velocity now */
float vbus; /* VBUS now */
};
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index 5319b14..790c76a 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -121,6 +121,16 @@ config INDUSTRY_FOC_ALIGN
---help---
Enable support for angle sensor alignment routine (zero offset + direction)
+if INDUSTRY_FOC_ALIGN
+
+config INDUSTRY_FOC_ALIGN_INDEX
+ bool "FOC alignment index search support"
+ default n
+ ---help---
+ Enable support for sensor alignment index search
+
+endif # INDUSTRY_FOC_ALIGN
+
config INDUSTRY_FOC_IDENT
bool "FOC motor identification routine"
default n
diff --git a/industry/foc/fixed16/foc_align.c b/industry/foc/fixed16/foc_align.c
index 442fda6..0f0ecbe 100644
--- a/industry/foc/fixed16/foc_align.c
+++ b/industry/foc/fixed16/foc_align.c
@@ -56,6 +56,12 @@
#define IDLE_STEPS (500)
+/* Index alignment */
+
+#define ALIGN_INDEX_ANGLE_SIGN (-b16muli(b16PI, 2))
+#define ALIGN_INDEX_ANGLE_STEP (ftob16(0.001f))
+#define ALIGN_INDEX_ANGLE_NOZERO (ftob16(0.1f))
+
/****************************************************************************
* Private Data Types
****************************************************************************/
@@ -65,6 +71,9 @@
enum foc_align_run_stage_e
{
FOC_ALIGN_RUN_INIT,
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ FOC_ALIGN_RUN_INDEX,
+#endif
FOC_ALIGN_RUN_OFFSET,
FOC_ALIGN_RUN_DIR,
FOC_ALIGN_RUN_IDLE,
@@ -80,6 +89,10 @@ struct foc_align_b16_s
int cntr;
int stage;
int dir_step;
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ int index_step;
+ b16_t index_angle;
+#endif
b16_t dir_angle;
b16_t angle_last;
b16_t angle_now;
@@ -116,6 +129,148 @@ struct foc_routine_ops_b16_s g_foc_routine_align_b16 =
* Private Functions
****************************************************************************/
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+/****************************************************************************
+ * Name: foc_align_index_run_b16
+ *
+ * Description:
+ * Run index 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_index_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)
+{
+ static b16_t dir = DIR_CW_B16;
+ b16_t sign = 0;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get mechanical angles */
+
+ align->angle_last = align->angle_now;
+ align->angle_now = in->angle_m;
+
+ /* Normalize angle to <-M_PI, M_PI> range */
+
+ align->angle_now = align->angle_now - b16PI;
+ angle_norm_2pi_b16(&align->angle_now, -b16PI, b16PI);
+
+ /* The product of the previous angle with an angle now gives us
+ * information about the encoder overflow.
+ * This procedure assumes that the encoder index signal resets the
+ * encoder value to zero.
+ */
+
+ sign = b16mulb16(align->angle_now, align->angle_last);
+
+ /* Move the motor shaft to approach index signal from both direction.
+ * After that, the encoder zero should be aligned with the encoder index.
+ */
+
+ if (align->index_step == 0)
+ {
+ /* Move the motor shaft away from zero */
+
+ align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
+
+ if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
+ align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
+ {
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 1)
+ {
+ /* Move the motor shaft to zero in positive direction */
+
+ align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
+
+ if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
+ {
+ dir = b16mulb16(DIR_CCW_B16, dir);
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 2)
+ {
+ /* Move the motor shaft away from zero */
+
+ align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
+
+ if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
+ align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
+ {
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 3)
+ {
+ /* Move the motor shaft to zero in negative direction */
+
+ align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
+
+ if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
+ {
+ dir = b16mulb16(DIR_CCW_B16, dir);
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 4)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+ }
+
+ /* Get output */
+
+ if (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;
+
+ /* Reset data */
+
+ align->angle_last = 0;
+ align->angle_now = 0;
+ align->cntr = 0;
+ }
+ 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 = align->index_angle;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+ }
+
+ return ret;
+}
+#endif
+
/****************************************************************************
* Name: foc_align_offset_run_b16
*
@@ -711,6 +866,29 @@ int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
break;
}
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ case FOC_ALIGN_RUN_INDEX:
+ {
+ /* Align zero procedure */
+
+ ret = foc_align_index_run_b16(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN INDEX done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+#endif
+
case FOC_ALIGN_RUN_OFFSET:
{
/* Align zero procedure */
diff --git a/industry/foc/float/foc_align.c b/industry/foc/float/foc_align.c
index 6c3c266..a7ff85a 100644
--- a/industry/foc/float/foc_align.c
+++ b/industry/foc/float/foc_align.c
@@ -56,6 +56,12 @@
#define IDLE_STEPS (500)
+/* Index alignment */
+
+#define ALIGN_INDEX_ANGLE_SIGN (-2.0f*M_PI)
+#define ALIGN_INDEX_ANGLE_STEP (0.001f)
+#define ALIGN_INDEX_ANGLE_NOZERO (0.1f)
+
/****************************************************************************
* Private Data Types
****************************************************************************/
@@ -65,6 +71,9 @@
enum foc_align_run_stage_e
{
FOC_ALIGN_RUN_INIT,
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ FOC_ALIGN_RUN_INDEX,
+#endif
FOC_ALIGN_RUN_OFFSET,
FOC_ALIGN_RUN_DIR,
FOC_ALIGN_RUN_IDLE,
@@ -80,6 +89,10 @@ struct foc_align_f32_s
int cntr;
int stage;
int dir_step;
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ int index_step;
+ float index_angle;
+#endif
float dir_angle;
float angle_last;
float angle_now;
@@ -116,6 +129,148 @@ struct foc_routine_ops_f32_s g_foc_routine_align_f32 =
* Private Functions
****************************************************************************/
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+/****************************************************************************
+ * Name: foc_align_index_run_f32
+ *
+ * Description:
+ * Run index 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_index_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)
+{
+ static float dir = DIR_CW;
+ float sign = 0.0f;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get mechanical angles */
+
+ align->angle_last = align->angle_now;
+ align->angle_now = in->angle_m;
+
+ /* Normalize angle to <-M_PI, M_PI> range */
+
+ align->angle_now = align->angle_now - M_PI;
+ angle_norm_2pi(&align->angle_now, -M_PI, M_PI);
+
+ /* The product of the previous angle with an angle now gives us
+ * information about the encoder overflow.
+ * This procedure assumes that the encoder index signal resets the
+ * encoder value to zero.
+ */
+
+ sign = align->angle_now * align->angle_last;
+
+ /* Move the motor shaft to approach index signal from both direction.
+ * After that, the encoder zero should be aligned with the encoder index.
+ */
+
+ if (align->index_step == 0)
+ {
+ /* Move the motor shaft away from zero */
+
+ align->index_angle += dir * ALIGN_INDEX_ANGLE_STEP;
+
+ if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
+ align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
+ {
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 1)
+ {
+ /* Move the motor shaft to zero in positive direction */
+
+ align->index_angle += dir * ALIGN_INDEX_ANGLE_STEP;
+
+ if (sign < 0.0f && sign < ALIGN_INDEX_ANGLE_SIGN)
+ {
+ dir = DIR_CCW * dir;
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 2)
+ {
+ /* Move the motor shaft away from zero */
+
+ align->index_angle += dir * ALIGN_INDEX_ANGLE_STEP;
+
+ if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
+ align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
+ {
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 3)
+ {
+ /* Move the motor shaft to zero in negative direction */
+
+ align->index_angle += dir * ALIGN_INDEX_ANGLE_STEP;
+
+ if (sign < 0.0f && sign < ALIGN_INDEX_ANGLE_SIGN)
+ {
+ dir = DIR_CCW * dir;
+ align->index_step += 1;
+ }
+ }
+
+ else if (align->index_step == 4)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+ }
+
+ /* Get output */
+
+ if (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;
+
+ /* Reset data */
+
+ align->angle_last = 0.0f;
+ align->angle_now = 0.0f;
+ align->cntr = 0;
+ }
+ 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 = align->index_angle;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ align->cntr += 1;
+ }
+
+ return ret;
+}
+#endif
+
/****************************************************************************
* Name: foc_align_offset_run_f32
*
@@ -711,6 +866,29 @@ int foc_routine_align_run_f32(FAR foc_routine_f32_t *r,
break;
}
+#ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
+ case FOC_ALIGN_RUN_INDEX:
+ {
+ /* Align zero procedure */
+
+ ret = foc_align_index_run_f32(a, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("ALIGN INDEX done!\n");
+
+ a->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+#endif
+
case FOC_ALIGN_RUN_OFFSET:
{
/* Align zero procedure */