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