examples/foc: add support for sensor alignment
This commit is contained in:
raiden00pl
Xiang Xiao
parent
b30f3329f6
commit
c771942d8e
@ -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
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
****************************************************************************/
|
||||
@ -581,6 +669,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
|
||||
{
|
||||
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
|
||||
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;
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
****************************************************************************/
|
||||
@ -567,6 +655,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
|
||||
{
|
||||
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
|
||||
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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -705,6 +869,9 @@ 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;
|
||||
#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 */
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user