From 54c0b9a9d87ecca0ba95c816b19a300242d8f932 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 6 Nov 2021 18:59:01 +0100 Subject: [PATCH] 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 409b33432..320d8a8f0 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 892e5e0af..fc3d68640 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 5319b142b..790c76af2 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 442fda63c..0f0ecbee5 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 6c3c266c2..a7ff85aec 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 */