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 <[email protected]> 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; +}
