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 c771942d8ed80189f1debcd5ad0851431315ddc4 Author: raiden00pl <[email protected]> AuthorDate: Sun Oct 31 22:09:21 2021 +0100 examples/foc: add support for sensor alignment --- examples/foc/Kconfig | 18 ++++ examples/foc/foc_cfg.h | 11 +++ examples/foc/foc_motor_b16.c | 188 +++++++++++++++++++++++++++++++++++++++- examples/foc/foc_motor_b16.h | 9 ++ examples/foc/foc_motor_f32.c | 201 +++++++++++++++++++++++++++++++++++++++++-- examples/foc/foc_motor_f32.h | 8 ++ examples/foc/foc_thr.h | 3 + 7 files changed, 429 insertions(+), 9 deletions(-) diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 3c1e2c7..26995f1 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -84,6 +84,7 @@ config EXAMPLES_FOC_SENSORLESS config EXAMPLES_FOC_SENSORED bool "FOC example sensored configuration" + select EXAMPLES_FOC_HAVE_ALIGN endchoice # @@ -271,6 +272,23 @@ config EXAMPLES_FOC_RAMP_DEC int "FOC velocity ramp acc [x1000]" default 0 +config EXAMPLES_FOC_HAVE_ALIGN + bool "FOC example motor alignment support" + select INDUSTRY_FOC_ALIGN + default n + +if EXAMPLES_FOC_HAVE_ALIGN + +config EXAMPLES_FOC_ALIGN_VOLT + int "EXAMPLES_FOC_ALIGN_VOLT (x1000)" + default 0 + +config EXAMPLES_FOC_ALIGN_SEC + int "EXAMPLES_FOC_ALIGN_SEC (x1000)" + default 0 + +endif # EXAMPLES_FOC_HAVE_ALIGN + endmenu # FOC controller parameters endif # EXAMPLES_FOC diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index c67107b..138eef8 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -104,4 +104,15 @@ # define FOC_MODEL_INDQ (0.0002f) #endif +/* Motor alignment configuration */ + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN +# if CONFIG_EXAMPLES_FOC_ALIGN_VOLT == 0 +# error +# endif +# if CONFIG_EXAMPLES_FOC_ALIGN_SEC == 0 +# error +# endif +#endif + #endif /* __EXAMPLES_FOC_FOC_CFG_H */ diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index e670a7e..1f67d20 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -44,6 +44,94 @@ * Private Functions ****************************************************************************/ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN +/**************************************************************************** + * Name: foc_align_dir_cb + ****************************************************************************/ + +static int foc_align_zero_cb(FAR void *priv, b16_t offset) +{ + FAR foc_angle_b16_t *angle = (FAR foc_angle_b16_t *)priv; + + DEBUGASSERT(angle); + + UNUSED(offset); + + return foc_angle_zero_b16(angle); +} + +/**************************************************************************** + * Name: foc_align_dir_cb + ****************************************************************************/ + +static int foc_align_dir_cb(FAR void *priv, b16_t dir) +{ + FAR foc_angle_b16_t *angle = (FAR foc_angle_b16_t *)priv; + + DEBUGASSERT(angle); + + return foc_angle_dir_b16(angle, dir); +} + +/**************************************************************************** + * Name: foc_motor_align + ****************************************************************************/ + +static int foc_motor_align(FAR struct foc_motor_b16_s *motor, FAR bool *done) +{ + struct foc_routine_in_b16_s in; + struct foc_routine_out_b16_s out; + struct foc_routine_aling_final_b16_s final; + int ret = OK; + + /* Get input */ + + in.foc_state = &motor->foc_state; + in.angle = motor->angle_now; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + in.vel = motor->vel.now; +#endif + in.vbus = motor->vbus; + + /* Run align procedure */ + + ret = foc_routine_run_b16(&motor->align, &in, &out); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_run_b16 failed %d!\n", ret); + goto errout; + } + + if (ret == FOC_ROUTINE_RUN_DONE) + { + ret = foc_routine_final_b16(&motor->align, &final); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_final_b16 failed %d!\n", ret); + goto errout; + } + + PRINTF("Aling results:\n"); + PRINTF(" dir = %.2f\n", b16tof(final.dir)); + PRINTF(" offset = %.2f\n", b16tof(final.offset)); + + *done = true; + } + + /* Copy output */ + + motor->dq_ref.d = out.dq_ref.d; + motor->dq_ref.q = out.dq_ref.q; + motor->vdq_comp.d = out.vdq_comp.d; + motor->vdq_comp.q = out.vdq_comp.q; + motor->angle_now = out.angle; + motor->foc_mode = out.foc_mode; + +errout: + return ret; +} +#endif + /**************************************************************************** * Name: foc_runmode_init ****************************************************************************/ @@ -580,9 +668,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, FAR struct foc_ctrl_env_s *envp) { #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - struct foc_openloop_cfg_b16_s ol_cfg; + struct foc_openloop_cfg_b16_s ol_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + struct foc_routine_align_cfg_b16_s align_cfg; #endif - int ret = OK; + int ret = OK; DEBUGASSERT(motor); DEBUGASSERT(envp); @@ -612,10 +703,44 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, foc_angle_cfg_b16(&motor->openloop, &ol_cfg); #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + /* Initialize motor alignment routine */ + + ret = foc_routine_init_b16(&motor->align, &g_foc_routine_align_b16); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_init_b16 failed %d!\n", ret); + goto errout; + } + + /* Initialize motor alignment data */ + + align_cfg.volt = ftob16(CONFIG_EXAMPLES_FOC_ALIGN_VOLT / 1000.0f); + align_cfg.offset_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \ + CONFIG_EXAMPLES_FOC_ALIGN_SEC / 1000); + + /* Connect align callbacks */ + + align_cfg.cb.zero = foc_align_zero_cb; + align_cfg.cb.dir = foc_align_dir_cb; + + /* TODO: Connect align callbacks private data */ + + align_cfg.cb.priv = NULL; + + ret = foc_routine_cfg_b16(&motor->align, &align_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_cfg_b16 failed %d!\n", ret); + goto errout; + } +#endif + /* Initialize controller state */ motor->ctrl_state = FOC_CTRL_STATE_INIT; +errout: return ret; } @@ -671,7 +796,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) /* Update open-loop angle handler */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL ain.vel = motor->vel.set; +#endif ain.angle = motor->angle_now; ain.dir = motor->dir; @@ -681,7 +808,32 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) /* Store open-loop angle */ motor->angle_ol = aout.angle; +#endif + +#ifdef CONFIG_EXAMPLES_FOC_SENSORED + /* Handle angle from sensor */ + + if (aout.type == FOC_ANGLE_TYPE_ELE) + { + /* Store electrical angle */ + + motor->angle_el = aout.angle; + } + + else if (aout.type == FOC_ANGLE_TYPE_MECH) + { + /* TODO */ + + ASSERT(0); + } + + else + { + ASSERT(0); + } +#endif /* CONFIG_EXAMPLES_FOC_SENSORED */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Get phase angle now */ if (motor->openloop_now == true) @@ -691,9 +843,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) else #endif { - /* TODO: get phase angle from observer or sensor */ + /* Get phase angle from observer or sensor */ - ASSERT(0); + motor->angle_now = motor->angle_el; } #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP @@ -709,6 +861,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) /* TODO: velocity observer or sensor */ } +errout: return ret; } @@ -719,6 +872,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) int foc_motor_control(FAR struct foc_motor_b16_s *motor) { int ret = OK; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + bool align_done = false; +#endif DEBUGASSERT(motor); @@ -736,6 +892,30 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor) break; } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + case FOC_CTRL_STATE_ALIGN: + { + /* Run motor align procedure */ + + ret = foc_motor_align(motor, &align_done); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_align failed %d!\n", ret); + goto errout; + } + + if (align_done == true) + { + /* Next state */ + + motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + } + + break; + } +#endif + case FOC_CTRL_STATE_RUN_INIT: { /* Initialize run controller mode */ diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index 34bcc37..aae0920 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -34,6 +34,10 @@ #include "industry/foc/fixed16/foc_ramp.h" #include "industry/foc/fixed16/foc_angle.h" #include "industry/foc/fixed16/foc_velocity.h" + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN +# include "industry/foc/fixed16/foc_align.h" +#endif #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM # include "industry/foc/fixed16/foc_model.h" #endif @@ -71,6 +75,8 @@ struct foc_motor_b16_s int ctrl_state; /* Controller state */ b16_t vbus; /* Power bus voltage */ b16_t angle_now; /* Phase angle now */ + b16_t angle_m; /* Motor mechanical angle */ + b16_t angle_el; /* Motor electrical angle */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ struct foc_setpoint_b16_s torq; /* Torque setpoint */ #endif @@ -90,6 +96,9 @@ struct foc_motor_b16_s struct foc_mq_s mq; /* MQ data */ struct foc_state_b16_s foc_state; /* FOC controller sate */ struct foc_ramp_b16_s ramp; /* Velocity ramp data */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + struct foc_routine_b16_s align; /* Alignment routine */ +#endif #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM struct foc_model_b16_s model; /* Model handler */ struct foc_model_state_b16_s model_state; /* PMSM model state */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 3dd3219..5495338 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -44,6 +44,94 @@ * Private Functions ****************************************************************************/ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN +/**************************************************************************** + * Name: foc_align_dir_cb + ****************************************************************************/ + +static int foc_align_zero_cb(FAR void *priv, float offset) +{ + FAR foc_angle_f32_t *angle = (FAR foc_angle_f32_t *)priv; + + DEBUGASSERT(angle); + + UNUSED(offset); + + return foc_angle_zero_f32(angle); +} + +/**************************************************************************** + * Name: foc_align_dir_cb + ****************************************************************************/ + +static int foc_align_dir_cb(FAR void *priv, float dir) +{ + FAR foc_angle_f32_t *angle = (FAR foc_angle_f32_t *)priv; + + DEBUGASSERT(angle); + + return foc_angle_dir_f32(angle, dir); +} + +/**************************************************************************** + * Name: foc_motor_align + ****************************************************************************/ + +static int foc_motor_align(FAR struct foc_motor_f32_s *motor, FAR bool *done) +{ + struct foc_routine_in_f32_s in; + struct foc_routine_out_f32_s out; + struct foc_routine_aling_final_f32_s final; + int ret = OK; + + /* Get input */ + + in.foc_state = &motor->foc_state; + in.angle = motor->angle_now; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + in.vel = motor->vel.now; +#endif + in.vbus = motor->vbus; + + /* Run align procedure */ + + ret = foc_routine_run_f32(&motor->align, &in, &out); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_run_f32 failed %d!\n", ret); + goto errout; + } + + if (ret == FOC_ROUTINE_RUN_DONE) + { + ret = foc_routine_final_f32(&motor->align, &final); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_final_f32 failed %d!\n", ret); + goto errout; + } + + PRINTF("Aling results:\n"); + PRINTF(" dir = %.2f\n", final.dir); + PRINTF(" offset = %.2f\n", final.offset); + + *done = true; + } + + /* Copy output */ + + motor->dq_ref.d = out.dq_ref.d; + motor->dq_ref.q = out.dq_ref.q; + motor->vdq_comp.d = out.vdq_comp.d; + motor->vdq_comp.q = out.vdq_comp.q; + motor->angle_now = out.angle; + motor->foc_mode = out.foc_mode; + +errout: + return ret; +} +#endif + /**************************************************************************** * Name: foc_runmode_init ****************************************************************************/ @@ -566,9 +654,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, FAR struct foc_ctrl_env_s *envp) { #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - struct foc_openloop_cfg_f32_s ol_cfg; + struct foc_openloop_cfg_f32_s ol_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + struct foc_routine_align_cfg_f32_s align_cfg; #endif - int ret = OK; + int ret = OK; DEBUGASSERT(motor); DEBUGASSERT(envp); @@ -598,10 +689,44 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, foc_angle_cfg_f32(&motor->openloop, &ol_cfg); #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + /* Initialize motor alignment routine */ + + ret = foc_routine_init_f32(&motor->align, &g_foc_routine_align_f32); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_init_f32 failed %d!\n", ret); + goto errout; + } + + /* Initialize motor alignment data */ + + align_cfg.volt = (CONFIG_EXAMPLES_FOC_ALIGN_VOLT / 1000.0f); + align_cfg.offset_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \ + CONFIG_EXAMPLES_FOC_ALIGN_SEC / 1000); + + /* Connect align callbacks */ + + align_cfg.cb.zero = foc_align_zero_cb; + align_cfg.cb.dir = foc_align_dir_cb; + + /* TODO: Connect align callbacks private data */ + + align_cfg.cb.priv = NULL; + + ret = foc_routine_cfg_f32(&motor->align, &align_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_cfg_f32 failed %d!\n", ret); + goto errout; + } +#endif + /* Initialize controller state */ motor->ctrl_state = FOC_CTRL_STATE_INIT; +errout: return ret; } @@ -615,6 +740,17 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor) DEBUGASSERT(motor); +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + /* Deinitialize motor alignment routine */ + + ret = foc_routine_deinit_f32(&motor->align); + if (ret < 0) + { + PRINTFV("ERROR: foc_routine_deinit_f32 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Deinitialize PMSM model */ @@ -657,7 +793,9 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) /* Update open-loop angle handler */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL ain.vel = motor->vel.set; +#endif ain.angle = motor->angle_now; ain.dir = motor->dir; @@ -667,7 +805,32 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) /* Store open-loop angle */ motor->angle_ol = aout.angle; +#endif + +#ifdef CONFIG_EXAMPLES_FOC_SENSORED + /* Handle angle from sensor */ + + if (aout.type == FOC_ANGLE_TYPE_ELE) + { + /* Store electrical angle */ + + motor->angle_el = aout.angle; + } + + else if (aout.type == FOC_ANGLE_TYPE_MECH) + { + /* TODO */ + ASSERT(0); + } + + else + { + ASSERT(0); + } +#endif /* CONFIG_EXAMPLES_FOC_SENSORED */ + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP /* Get phase angle now */ if (motor->openloop_now == true) @@ -677,9 +840,9 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) else #endif { - /* TODO: get phase angle from observer or sensor */ + /* Get phase angle from observer or sensor */ - ASSERT(0); + motor->angle_now = motor->angle_el; } #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP @@ -695,6 +858,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) /* TODO: velocity observer or sensor */ } +errout: return ret; } @@ -704,7 +868,10 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) int foc_motor_control(FAR struct foc_motor_f32_s *motor) { - int ret = OK; + int ret = OK; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + bool align_done = false; +#endif DEBUGASSERT(motor); @@ -722,6 +889,30 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor) break; } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + case FOC_CTRL_STATE_ALIGN: + { + /* Run motor align procedure */ + + ret = foc_motor_align(motor, &align_done); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_align failed %d!\n", ret); + goto errout; + } + + if (align_done == true) + { + /* Next state */ + + motor->ctrl_state += 1; + motor->foc_mode = FOC_HANDLER_MODE_IDLE; + } + + break; + } +#endif + case FOC_CTRL_STATE_RUN_INIT: { /* Initialize run controller mode */ diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index f6e2c0b..80f2fa9 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -35,6 +35,9 @@ #include "industry/foc/float/foc_angle.h" #include "industry/foc/float/foc_velocity.h" +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN +# include "industry/foc/float/foc_align.h" +#endif #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM # include "industry/foc/float/foc_model.h" #endif @@ -72,6 +75,8 @@ struct foc_motor_f32_s int ctrl_state; /* Controller state */ float vbus; /* Power bus voltage */ float angle_now; /* Phase angle now */ + float angle_m; /* Motor mechanical angle */ + float angle_el; /* Motor electrical angle */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ struct foc_setpoint_f32_s torq; /* Torque setpoint */ #endif @@ -91,6 +96,9 @@ struct foc_motor_f32_s struct foc_mq_s mq; /* MQ data */ struct foc_state_f32_s foc_state; /* FOC controller sate */ struct foc_ramp_f32_s ramp; /* Velocity ramp data */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + struct foc_routine_f32_s align; /* Alignment routine */ +#endif #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM struct foc_model_f32_s model; /* Model handler */ struct foc_model_state_f32_s model_state; /* PMSM model state */ diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h index c824f68..c33dfef 100644 --- a/examples/foc/foc_thr.h +++ b/examples/foc/foc_thr.h @@ -80,6 +80,9 @@ enum foc_controller_state_e { FOC_CTRL_STATE_INVALID = 0, FOC_CTRL_STATE_INIT, +#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN + FOC_CTRL_STATE_ALIGN, +#endif FOC_CTRL_STATE_RUN_INIT, FOC_CTRL_STATE_RUN, FOC_CTRL_STATE_IDLE
