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

examples/foc: refactor control loop

Browse Source 
1. get FOC device state at the beginning of the loop
2. simplify if-else block
This commit is contained in:
raiden00pl 2023-10-20 16:30:40 +02:00 committed by Xiang Xiao
parent 388256b7d7
commit 73ab7dedd4
2 changed files with 164 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
examples/foc/foc_fixed16_thr.c
View File

@ -339,6 +339,18 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
while (motor.mq.quit == false)
{
if (motor.mq.start == true)
{
/* Get FOC device state */
ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
}
PRINTFV("foc_fixed16_thr %d %d\n", envp->id, motor.time);
/* Handle mqueue */
@ -376,121 +388,111 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
motor.startstop = false;
}
/* Run control logic if controller started */
/* Ignore control logic if controller not started yet */
if (motor.mq.start == true)
if (motor.mq.start == false)
{
/* Get FOC device state */
ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
usleep(1000);
continue;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Get model state */
/* Get model state */
ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
#endif
/* Handle controller state */
/* Handle controller state */
ret = foc_dev_state_handle(&dev, &motor.fault);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
ret = foc_dev_state_handle(&dev, &motor.fault);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
/* Get motor state */
/* Get motor state */
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
/* Motor control */
/* Motor control */
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
/* Run FOC */
/* Run FOC */
ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
goto errout;
}
ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
goto errout;
}
#ifdef FOC_STATE_PRINT_PRE
/* Print state if configured */
/* Print state if configured */
if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Feed FOC model with data */
/* Feed FOC model with data */
foc_model_run_b16(&motor.model,
ftob16(FOC_MODEL_LOAD),
&motor.foc_state.vab);
foc_model_run_b16(&motor.model,
ftob16(FOC_MODEL_LOAD),
&motor.foc_state.vab);
#endif
/* Set FOC device parameters */
/* Set FOC device parameters */
ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_params_set failed %d!\n", ret);
goto errout;
}
ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_params_set failed %d!\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/* Capture nxscope samples */
/* Capture nxscope samples */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_fixed16_nxscope(envp->nxs, &motor, &dev);
}
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_fixed16_nxscope(envp->nxs, &motor, &dev);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
/* Handle nxscope work */
/* Handle nxscope work */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0)
{
foc_nxscope_work(envp->nxs);
}
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0)
{
foc_nxscope_work(envp->nxs);
}
#endif
/* Terminate control thread */
/* Terminate control thread */
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
{
PRINTF("TERMINATE CTRL THREAD\n");
goto errout;
}
}
else
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
{
usleep(1000);
PRINTF("TERMINATE CTRL THREAD\n");
goto errout;
}
/* Increase counter */

162
examples/foc/foc_float_thr.c
View File

@ -352,6 +352,18 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
while (motor.mq.quit == false)
{
if (motor.mq.start == true)
{
/* Get FOC device state */
ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
}
PRINTFV("foc_float_thr %d %d\n", envp->id, motor.time);
/* Handle mqueue */
@ -389,121 +401,111 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
motor.startstop = false;
}
/* Run control logic if controller started */
/* Ignore control logic if controller not started yet */
if (motor.mq.start == true)
if (motor.mq.start == false)
{
/* Get FOC device state */
ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
usleep(1000);
continue;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Get model state */
/* Get model state */
ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
#endif
/* Handle controller state */
/* Handle controller state */
ret = foc_dev_state_handle(&dev, &motor.fault);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
ret = foc_dev_state_handle(&dev, &motor.fault);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
/* Get motor state */
/* Get motor state */
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
/* Motor control */
/* Motor control */
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
/* Run FOC */
/* Run FOC */
ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
goto errout;
}
ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
goto errout;
}
#ifdef FOC_STATE_PRINT_PRE
/* Print state if configured */
/* Print state if configured */
if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Feed FOC model with data */
/* Feed FOC model with data */
foc_model_run_f32(&motor.model,
FOC_MODEL_LOAD,
&motor.foc_state.vab);
foc_model_run_f32(&motor.model,
FOC_MODEL_LOAD,
&motor.foc_state.vab);
#endif
/* Set FOC device parameters */
/* Set FOC device parameters */
ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
goto errout;
}
ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/* Capture nxscope samples */
/* Capture nxscope samples */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_float_nxscope(envp->nxs, &motor, &dev);
}
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_float_nxscope(envp->nxs, &motor, &dev);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
/* Handle nxscope work */
/* Handle nxscope work */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0)
{
foc_nxscope_work(envp->nxs);
}
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0)
{
foc_nxscope_work(envp->nxs);
}
#endif
/* Terminate control thread */
/* Terminate control thread */
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
{
PRINTF("TERMINATE CTRL THREAD\n");
goto errout;
}
}
else
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
{
usleep(1000);
PRINTF("TERMINATE CTRL THREAD\n");
goto errout;
}
/* Increase counter */