You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2022/11/03 18:07:24 UTC
[incubator-nuttx-apps] 03/06: examples/foc: simplify control thread configuration
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 2c5dc1e4a2d3c838f4209df947c0752355667c3a
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Oct 22 18:15:23 2022 +0200
examples/foc: simplify control thread configuration
---
examples/foc/foc_main.c | 19 +------------------
examples/foc/foc_motor_b16.c | 37 +++++++++++++++++++------------------
examples/foc/foc_motor_f32.c | 37 +++++++++++++++++++------------------
examples/foc/foc_thr.h | 29 +++++------------------------
4 files changed, 44 insertions(+), 78 deletions(-)
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 7ab3eb3b3..4854c5584 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -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))
{
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 8a4083a0f..29eb49e76 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -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;
}
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index c123881cf..587744757 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -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;
}
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index 18889c106..f08a81437 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -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 */
};
/****************************************************************************