sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

examples/foc: add support for sensor alignment

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 22:09:21 +01:00 committed by Xiang Xiao
parent b30f3329f6
commit c771942d8e
7 changed files with 429 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
examples/foc/Kconfig
View File

@ -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

11
examples/foc/foc_cfg.h
View File

@ -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 */

188
examples/foc/foc_motor_b16.c
View File

@ -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
int ret = OK;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_b16_s align_cfg;
#endif
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 */

9
examples/foc/foc_motor_b16.h
View File

@ -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 */

201
examples/foc/foc_motor_f32.c
View File

@ -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
int ret = OK;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_f32_s align_cfg;
#endif
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 */

8
examples/foc/foc_motor_f32.h
View File

@ -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 */

3
examples/foc/foc_thr.h
View File

@ -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