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

examples/foc: simplify control thread configuration

Browse Source
This commit is contained in:
raiden00pl 2022-10-22 18:15:23 +02:00 committed by Xiang Xiao
parent b1f91528ec
commit 2c5dc1e4a2
4 changed files with 44 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
examples/foc/foc_main.c
View File

@ -343,24 +343,7 @@ int main(int argc, char *argv[])
{
/* Get configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc[i].qparam = g_args.cfg.qparam;
#endif
foc[i].fmode = g_args.cfg.fmode;
foc[i].mmode = g_args.cfg.mmode;
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
foc[i].foc_pi_kp = g_args.cfg.foc_pi_kp;
foc[i].foc_pi_ki = g_args.cfg.foc_pi_ki;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
foc[i].torqmax = g_args.cfg.torqmax;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
foc[i].velmax = g_args.cfg.velmax;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
foc[i].posmax = g_args.cfg.posmax;
#endif
foc[i].cfg = &g_args.cfg;
if (g_args.en & (1 << i))
{

37
examples/foc/foc_motor_b16.c
View File

@ -232,7 +232,7 @@ static int foc_runmode_init(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
switch (motor->envp->fmode)
switch (motor->envp->cfg->fmode)
{
case FOC_FMODE_IDLE:
{
@ -254,7 +254,7 @@ static int foc_runmode_init(FAR struct foc_motor_b16_s *motor)
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->fmode);
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->cfg->fmode);
ret = -EINVAL;
goto errout;
}
@ -346,10 +346,10 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
/* Get PI controller configuration */
ctrl_cfg.id_kp = ftob16(motor->envp->foc_pi_kp / 1000.0f);
ctrl_cfg.id_ki = ftob16(motor->envp->foc_pi_ki / 1000.0f);
ctrl_cfg.iq_kp = ftob16(motor->envp->foc_pi_kp / 1000.0f);
ctrl_cfg.iq_ki = ftob16(motor->envp->foc_pi_ki / 1000.0f);
ctrl_cfg.id_kp = ftob16(motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.id_ki = ftob16(motor->envp->cfg->foc_pi_ki / 1000.0f);
ctrl_cfg.iq_kp = ftob16(motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.iq_ki = ftob16(motor->envp->cfg->foc_pi_ki / 1000.0f);
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
@ -431,7 +431,7 @@ static int foc_motor_torq(FAR struct foc_motor_b16_s *motor, uint32_t torq)
* NOTE: torqmax may not fit in b16_t so we can't use b16idiv()
*/
tmp1 = itob16(motor->envp->torqmax / 1000);
tmp1 = itob16(motor->envp->cfg->torqmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
motor->torq.des = b16muli(tmp2, torq);
@ -456,7 +456,7 @@ static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel)
* NOTE: velmax may not fit in b16_t so we can't use b16idiv()
*/
tmp1 = itob16(motor->envp->velmax / 1000);
tmp1 = itob16(motor->envp->cfg->velmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
motor->vel.des = b16muli(tmp2, vel);
@ -481,7 +481,7 @@ static int foc_motor_pos(FAR struct foc_motor_b16_s *motor, uint32_t pos)
* NOTE: posmax may not fit in b16_t so we can't use b16idiv()
*/
tmp1 = itob16(motor->envp->posmax / 1000);
tmp1 = itob16(motor->envp->cfg->posmax / 1000);
tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
motor->pos.des = b16muli(tmp2, pos);
@ -498,7 +498,7 @@ static int foc_motor_setpoint(FAR struct foc_motor_b16_s *motor, uint32_t sp)
{
int ret = OK;
switch (motor->envp->mmode)
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
@ -562,7 +562,8 @@ static int foc_motor_setpoint(FAR struct foc_motor_b16_s *motor, uint32_t sp)
default:
{
PRINTF("ERROR: unsupported ctrl mode %d\n", motor->envp->mmode);
PRINTF("ERROR: unsupported ctrl mode %d\n",
motor->envp->cfg->mmode);
ret = -EINVAL;
goto errout;
}
@ -718,7 +719,7 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
if (motor->openloop_now == true)
{
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->mmode != FOC_MMODE_VEL)
if (motor->envp->cfg->mmode != FOC_MMODE_VEL)
#endif
{
PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
@ -735,7 +736,7 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
/* Controller */
switch (motor->envp->mmode)
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
@ -782,7 +783,7 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
* NOTE: Id always set to 0
*/
q_ref = b16idiv(motor->envp->qparam, 1000);
q_ref = b16idiv(motor->envp->cfg->qparam, 1000);
d_ref = 0;
}
#endif
@ -1010,14 +1011,14 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
/* Initialize controller state */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
if (motor->envp->mmode == FOC_MMODE_ALIGN_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
}
else
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
if (motor->envp->mmode == FOC_MMODE_IDENT_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_IDENT;
}
@ -1246,7 +1247,7 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor)
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->mmode == FOC_MMODE_ALIGN_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}
@ -1275,7 +1276,7 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor)
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->mmode == FOC_MMODE_IDENT_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}

37
examples/foc/foc_motor_f32.c
View File

@ -232,7 +232,7 @@ static int foc_runmode_init(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
switch (motor->envp->fmode)
switch (motor->envp->cfg->fmode)
{
case FOC_FMODE_IDLE:
{
@ -254,7 +254,7 @@ static int foc_runmode_init(FAR struct foc_motor_f32_s *motor)
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->fmode);
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->cfg->fmode);
ret = -EINVAL;
goto errout;
}
@ -344,10 +344,10 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
/* Get PI controller configuration */
ctrl_cfg.id_kp = (motor->envp->foc_pi_kp / 1000.0f);
ctrl_cfg.id_ki = (motor->envp->foc_pi_ki / 1000.0f);
ctrl_cfg.iq_kp = (motor->envp->foc_pi_kp / 1000.0f);
ctrl_cfg.iq_ki = (motor->envp->foc_pi_ki / 1000.0f);
ctrl_cfg.id_kp = (motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.id_ki = (motor->envp->cfg->foc_pi_ki / 1000.0f);
ctrl_cfg.iq_kp = (motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.iq_ki = (motor->envp->cfg->foc_pi_ki / 1000.0f);
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
@ -425,7 +425,7 @@ static int foc_motor_torq(FAR struct foc_motor_f32_s *motor, uint32_t torq)
/* Update motor torque destination */
motor->torq.des = (torq * SETPOINT_ADC_SCALE *
motor->envp->torqmax / 1000.0f);
motor->envp->cfg->torqmax / 1000.0f);
return OK;
}
@ -443,7 +443,7 @@ static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
/* Update motor velocity destination */
motor->vel.des = (vel * SETPOINT_ADC_SCALE *
motor->envp->velmax / 1000.0f);
motor->envp->cfg->velmax / 1000.0f);
return OK;
}
@ -461,7 +461,7 @@ static int foc_motor_pos(FAR struct foc_motor_f32_s *motor, uint32_t pos)
/* Update motor position destination */
motor->pos.des = (pos * SETPOINT_ADC_SCALE *
motor->envp->posmax / 1000.0f);
motor->envp->cfg->posmax / 1000.0f);
return OK;
}
@ -475,7 +475,7 @@ static int foc_motor_setpoint(FAR struct foc_motor_f32_s *motor, uint32_t sp)
{
int ret = OK;
switch (motor->envp->mmode)
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
@ -539,7 +539,8 @@ static int foc_motor_setpoint(FAR struct foc_motor_f32_s *motor, uint32_t sp)
default:
{
PRINTF("ERROR: unsupported ctrl mode %d\n", motor->envp->mmode);
PRINTF("ERROR: unsupported ctrl mode %d\n",
motor->envp->cfg->mmode);
ret = -EINVAL;
goto errout;
}
@ -702,7 +703,7 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
if (motor->openloop_now == true)
{
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->mmode != FOC_MMODE_VEL)
if (motor->envp->cfg->mmode != FOC_MMODE_VEL)
# endif
{
PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
@ -719,7 +720,7 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
/* Controller */
switch (motor->envp->mmode)
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
@ -766,7 +767,7 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
* NOTE: Id always set to 0
*/
q_ref = (motor->envp->qparam / 1000.0f);
q_ref = (motor->envp->cfg->qparam / 1000.0f);
d_ref = 0.0f;
}
#endif
@ -991,14 +992,14 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
/* Initialize controller state */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
if (motor->envp->mmode == FOC_MMODE_ALIGN_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
}
else
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
if (motor->envp->mmode == FOC_MMODE_IDENT_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_IDENT;
}
@ -1227,7 +1228,7 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor)
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->mmode == FOC_MMODE_ALIGN_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}
@ -1256,7 +1257,7 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor)
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->mmode == FOC_MMODE_IDENT_ONLY)
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}

29
examples/foc/foc_thr.h
View File

@ -105,30 +105,11 @@ enum foc_controller_state_e
struct foc_ctrl_env_s
{
mqd_t mqd; /* Control msg queue */
int id; /* FOC device id */
int inst; /* Type specific instance counter */
int type; /* Controller type */
int fmode; /* FOC control mode */
int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
int qparam; /* Open-loop Q setting (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
uint32_t foc_pi_kp; /* FOC PI Kp (x1000) */
uint32_t foc_pi_ki; /* FOC PI Ki (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
uint32_t torqmax; /* Torque max (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
uint32_t velmax; /* Velocity max (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
uint32_t posmax; /* Position max (x1000) */
#endif
mqd_t mqd; /* Control msg queue */
int id; /* FOC device id */
int inst; /* Type specific instance counter */
int type; /* Controller type */
struct foc_thr_cfg_s *cfg; /* Control thread configuration */
};
/****************************************************************************