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 */ }; /****************************************************************************