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

examples/foc: move the common FOC dev logic from threads to one place

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 11:30:30 +01:00 committed by Xiang Xiao
parent e0ef3cecb9
commit 22ed7da99c
4 changed files with 281 additions and 269 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
examples/foc/foc_device.c
View File

@ -36,13 +36,14 @@
****************************************************************************/ ****************************************************************************/
/**************************************************************************** /****************************************************************************
* Name: foc_device_open * Name: foc_device_init
****************************************************************************/ ****************************************************************************/
int foc_device_open(FAR struct foc_device_s *dev, int id) int foc_device_init(FAR struct foc_device_s *dev, int id)
{ {
char devpath[32]; char devpath[32];
int ret = OK; int ret = OK;
struct foc_cfg_s cfg;
DEBUGASSERT(dev); DEBUGASSERT(dev);
@ -69,15 +70,33 @@ int foc_device_open(FAR struct foc_device_s *dev, int id)
goto errout; goto errout;
} }
/* Get FOC device configuration */
cfg.pwm_freq = (CONFIG_EXAMPLES_FOC_PWM_FREQ);
cfg.notifier_freq = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
/* Print FOC device configuration */
foc_cfg_print(&cfg);
/* Configure FOC device */
ret = foc_dev_setcfg(dev->fd, &cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setcfg %d!\n", ret);
goto errout;
}
errout: errout:
return ret; return ret;
} }
/**************************************************************************** /****************************************************************************
* Name: foc_device_close * Name: foc_device_deinit
****************************************************************************/ ****************************************************************************/
int foc_device_close(FAR struct foc_device_s *dev) int foc_device_deinit(FAR struct foc_device_s *dev)
{ {
int ret = OK; int ret = OK;
@ -98,3 +117,115 @@ int foc_device_close(FAR struct foc_device_s *dev)
return ret; return ret;
} }
/****************************************************************************
* Name: foc_device_start
****************************************************************************/
int foc_device_start(FAR struct foc_device_s *dev, bool state)
{
int ret = OK;
DEBUGASSERT(dev);
if (state == true)
{
ret = foc_dev_start(dev->fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
goto errout;
}
}
else
{
ret = foc_dev_stop(dev->fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
goto errout;
}
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_state_get
****************************************************************************/
int foc_dev_state_get(FAR struct foc_device_s *dev)
{
int ret = OK;
DEBUGASSERT(dev);
/* Get FOC state - blocking */
ret = foc_dev_getstate(dev->fd, &dev->state);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_params_set
****************************************************************************/
int foc_dev_params_set(FAR struct foc_device_s *dev)
{
int ret = OK;
DEBUGASSERT(dev);
/* Write FOC parameters */
ret = foc_dev_setparams(dev->fd, &dev->params);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_state_handle
****************************************************************************/
int foc_dev_state_handle(FAR struct foc_device_s *dev, FAR bool *flag)
{
int ret = OK;
DEBUGASSERT(dev);
DEBUGASSERT(flag);
if (dev->state.fault > 0)
{
PRINTF("FAULT = %d\n", dev->state.fault);
*flag = true;
/* Clear fault state */
ret = foc_dev_clearfault(dev->fd);
if (ret != OK)
{
goto errout;
}
}
else
{
*flag = false;
}
errout:
return ret;
}

8
examples/foc/foc_device.h
View File

@ -51,7 +51,11 @@ struct foc_device_s
* Public Function Prototypes * Public Function Prototypes
****************************************************************************/ ****************************************************************************/
int foc_device_open(FAR struct foc_device_s *dev, int id); int foc_device_init(FAR struct foc_device_s *dev, int id);
int foc_device_close(FAR struct foc_device_s *dev); int foc_device_deinit(FAR struct foc_device_s *dev);
int foc_device_start(FAR struct foc_device_s *dev, bool state);
int foc_dev_state_get(FAR struct foc_device_s *dev);
int foc_dev_params_set(FAR struct foc_device_s *dev);
int foc_dev_state_handle(FAR struct foc_device_s *dev, FAR bool *flag);
#endif /* __EXAMPLES_FOC_FOC_DEVICE_H */ #endif /* __EXAMPLES_FOC_FOC_DEVICE_H */

200
examples/foc/foc_fixed16_thr.c
View File

@ -64,8 +64,8 @@
struct foc_motor_b16_s struct foc_motor_b16_s
{ {
FAR struct foc_ctrl_env_s *envp; /* Thread env */ FAR struct foc_ctrl_env_s *envp; /* Thread env */
struct foc_device_s dev; /* FOC device */
bool fault; /* Fault flag */ bool fault; /* Fault flag */
bool startstop; /* Start/stop request */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */ bool openloop_now; /* Open-loop now */
b16_t angle_ol; /* Phase angle open-loop */ b16_t angle_ol; /* Phase angle open-loop */
@ -80,6 +80,7 @@ struct foc_motor_b16_s
b16_t dir; /* Motor's direction */ b16_t dir; /* Motor's direction */
b16_t per; /* Controller period in seconds */ b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */ b16_t iphase_adc; /* Iphase ADC scaling factor */
b16_t pwm_duty_max; /* PWM duty max */
dq_frame_b16_t dq_ref; /* DQ reference */ dq_frame_b16_t dq_ref; /* DQ reference */
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
foc_handler_b16_t handler; /* FOC controller */ foc_handler_b16_t handler; /* FOC controller */
@ -198,7 +199,6 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_pmsm_cfg_b16_s pmsm_cfg; struct foc_model_pmsm_cfg_b16_s pmsm_cfg;
#endif #endif
struct foc_cfg_s cfg;
int ret = OK; int ret = OK;
DEBUGASSERT(motor); DEBUGASSERT(motor);
@ -239,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* Get SVM3 modulation configuration */ /* Get SVM3 modulation configuration */
mod_cfg.pwm_duty_max = FOCDUTY_TO_FIXED16(motor->dev.info.hw_cfg.pwm_max); mod_cfg.pwm_duty_max = motor->pwm_duty_max;
#endif #endif
/* Configure FOC handler */ /* Configure FOC handler */
@ -274,24 +274,6 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
foc_model_cfg_b16(&motor->model, &pmsm_cfg); foc_model_cfg_b16(&motor->model, &pmsm_cfg);
#endif #endif
/* Get FOC device configuration */
cfg.pwm_freq = (CONFIG_EXAMPLES_FOC_PWM_FREQ);
cfg.notifier_freq = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
/* Print FOC device configuration */
foc_cfg_print(&cfg);
/* Configure FOC device */
ret = foc_dev_setcfg(motor->dev.fd, &cfg);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_setcfg %d!\n", ret);
goto errout;
}
errout: errout:
return ret; return ret;
} }
@ -308,13 +290,13 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
if (start == true) if (start == true)
{ {
/* Start device if VBUS data present */ /* Start motor if VBUS data present */
if (motor->mq.vbus > 0) if (motor->mq.vbus > 0)
{ {
/* Configure FOC device */ /* Configure motor controller */
PRINTF("Configure FOC device %d!\n", motor->envp->id); PRINTF("Configure motor %d!\n", motor->envp->id);
ret = foc_motor_configure(motor); ret = foc_motor_configure(motor);
if (ret < 0) if (ret < 0)
@ -323,30 +305,16 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
goto errout; goto errout;
} }
/* Start device */ /* Start/stop FOC dev request */
PRINTF("Start FOC device %d!\n", motor->envp->id); motor->startstop = true;
ret = foc_dev_start(motor->dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_start failed %d!\n", ret);
goto errout;
}
} }
} }
else else
{ {
/* Stop FOC device */ /* Start/stop FOC dev request */
PRINTF("Stop FOC device %d!\n", motor->envp->id); motor->startstop = true;
ret = foc_dev_stop(motor->dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_stop failed %d!\n", ret);
goto errout;
}
} }
errout: errout:
@ -576,7 +544,8 @@ errout:
* Name: foc_handler_run * Name: foc_handler_run
****************************************************************************/ ****************************************************************************/
static int foc_handler_run(FAR struct foc_motor_b16_s *motor) static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
FAR struct foc_device_s *dev)
{ {
struct foc_handler_input_b16_s input; struct foc_handler_input_b16_s input;
struct foc_handler_output_b16_s output; struct foc_handler_output_b16_s output;
@ -585,6 +554,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
int i = 0; int i = 0;
DEBUGASSERT(motor); DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* FOC device fault */ /* FOC device fault */
@ -606,7 +576,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
current[i] = b16muli(motor->iphase_adc, motor->dev.state.curr[i]); current[i] = b16muli(motor->iphase_adc, dev->state.curr[i]);
} }
/* Get input for FOC handler */ /* Get input for FOC handler */
@ -626,7 +596,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev.params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]); dev->params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
} }
/* Get FOC handler state */ /* Get FOC handler state */
@ -636,29 +606,19 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
return ret; return ret;
} }
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/**************************************************************************** /****************************************************************************
* Name: foc_dev_state_get * Name: foc_model_state_get
****************************************************************************/ ****************************************************************************/
static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor) static int foc_model_state_get(FAR struct foc_motor_b16_s *motor,
FAR struct foc_device_s *dev)
{ {
int ret = OK; int i = 0;
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
int i;
#endif
DEBUGASSERT(motor); DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* Get FOC state - blocking */
ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Get model state */ /* Get model state */
foc_model_state_b16(&motor->model, &motor->model_state); foc_model_state_b16(&motor->model, &motor->model_state);
@ -667,57 +627,12 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev.state.curr[i] = motor->model_state.curr_raw[i]; dev->state.curr[i] = motor->model_state.curr_raw[i];
}
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_params_set
****************************************************************************/
static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* Write FOC parameters */
ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_state_handle
****************************************************************************/
static int foc_dev_state_handle(FAR struct foc_motor_b16_s *motor)
{
DEBUGASSERT(motor);
if (motor->dev.state.fault > 0)
{
PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true;
}
else
{
motor->fault = false;
} }
return OK; return OK;
} }
#endif
#ifdef FOC_STATE_PRINT_PRE #ifdef FOC_STATE_PRINT_PRE
/**************************************************************************** /****************************************************************************
@ -806,13 +721,15 @@ static int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
motor->mq.app_state = handle->app_state; motor->mq.app_state = handle->app_state;
} }
/* Start/stop motor */ /* Start/stop controller */
if (motor->mq.start != handle->start) if (motor->mq.start != handle->start)
{ {
PRINTFV("Set start=%d for FOC driver %d!\n", PRINTFV("Set start=%d for FOC driver %d!\n",
handle->start, motor->envp->id); handle->start, motor->envp->id);
/* Start/stop motor controller */
ret = foc_motor_start(motor, handle->start); ret = foc_motor_start(motor, handle->start);
if (ret < 0) if (ret < 0)
{ {
@ -839,6 +756,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
{ {
struct foc_mq_s handle; struct foc_mq_s handle;
struct foc_motor_b16_s motor; struct foc_motor_b16_s motor;
struct foc_device_s dev;
int time = 0; int time = 0;
int ret = OK; int ret = OK;
@ -859,15 +777,19 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
/* Open FOC device as blocking */ /* Initialize FOC device as blocking */
ret = foc_device_open(&motor.dev, envp->id); ret = foc_device_init(&dev, envp->id);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_device_open failed %d!\n", ret); PRINTF("ERROR: foc_device_init failed %d!\n", ret);
goto errout; goto errout;
} }
/* Get PWM max duty */
motor.pwm_duty_max = FOCDUTY_TO_FIXED16(dev.info.hw_cfg.pwm_max);
/* Initialize controller mode */ /* Initialize controller mode */
ret = foc_mode_init(&motor); ret = foc_mode_init(&motor);
@ -909,40 +831,56 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
if (motor.startstop == true)
{
/* Start or stop device */
PRINTF("Start FOC device %d state=%d!\n",
motor.envp->id, motor.mq.start);
ret = foc_device_start(&dev, motor.mq.start);
if (ret < 0)
{
PRINTFV("ERROR: foc_device_start failed %d!\n", ret);
goto errout;
}
motor.startstop = false;
}
/* Run control logic if controller started */ /* Run control logic if controller started */
if (motor.mq.start == true) if (motor.mq.start == true)
{ {
/* Get FOC device state */ /* Get FOC device state */
ret = foc_dev_state_get(&motor); ret = foc_dev_state_get(&dev);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret); PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout; goto errout;
} }
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* 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;
}
#endif
/* Handle controller state */ /* Handle controller state */
ret = foc_dev_state_handle(&motor); 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;
} }
if (motor.dev.state.fault > 0)
{
/* Clear fault state */
ret = foc_dev_clearfault(motor.dev.fd);
if (ret != OK)
{
PRINTF("ERROR: foc_dev_clearfault failed %d!\n", ret);
goto errout;
}
}
/* Get motor state */ /* Get motor state */
ret = foc_motor_get(&motor); ret = foc_motor_get(&motor);
@ -963,7 +901,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Run FOC */ /* Run FOC */
ret = foc_handler_run(&motor); 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);
@ -989,7 +927,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Set FOC device parameters */ /* Set FOC device parameters */
ret = foc_dev_params_set(&motor); 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);
@ -1018,12 +956,12 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id); PRINTF("Stop FOC device %d!\n", envp->id);
/* Close FOC control device */ /* De-initialize FOC device */
ret = foc_device_close(&motor.dev); ret = foc_device_deinit(&dev);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret); PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret);
} }
PRINTF("foc_fixed16_thr %d exit\n", envp->id); PRINTF("foc_fixed16_thr %d exit\n", envp->id);

199
examples/foc/foc_float_thr.c
View File

@ -64,8 +64,8 @@
struct foc_motor_f32_s struct foc_motor_f32_s
{ {
FAR struct foc_ctrl_env_s *envp; /* Thread env */ FAR struct foc_ctrl_env_s *envp; /* Thread env */
struct foc_device_s dev; /* FOC device */
bool fault; /* Fault flag */ bool fault; /* Fault flag */
bool startstop; /* Start/stop request */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */ bool openloop_now; /* Open-loop now */
float angle_ol; /* Phase angle open-loop */ float angle_ol; /* Phase angle open-loop */
@ -80,6 +80,7 @@ struct foc_motor_f32_s
float dir; /* Motor's direction */ float dir; /* Motor's direction */
float per; /* Controller period in seconds */ float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */ float iphase_adc; /* Iphase ADC scaling factor */
float pwm_duty_max; /* PWM duty max */
dq_frame_f32_t dq_ref; /* DQ reference */ dq_frame_f32_t dq_ref; /* DQ reference */
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
foc_handler_f32_t handler; /* FOC controller */ foc_handler_f32_t handler; /* FOC controller */
@ -198,7 +199,6 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_pmsm_cfg_f32_s pmsm_cfg; struct foc_model_pmsm_cfg_f32_s pmsm_cfg;
#endif #endif
struct foc_cfg_s cfg;
int ret = OK; int ret = OK;
DEBUGASSERT(motor); DEBUGASSERT(motor);
@ -239,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 #ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* Get SVM3 modulation configuration */ /* Get SVM3 modulation configuration */
mod_cfg.pwm_duty_max = FOCDUTY_TO_FLOAT(motor->dev.info.hw_cfg.pwm_max); mod_cfg.pwm_duty_max = motor->pwm_duty_max;
#endif #endif
/* Configure FOC handler */ /* Configure FOC handler */
@ -274,24 +274,6 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
foc_model_cfg_f32(&motor->model, &pmsm_cfg); foc_model_cfg_f32(&motor->model, &pmsm_cfg);
#endif #endif
/* Get FOC device configuration */
cfg.pwm_freq = (CONFIG_EXAMPLES_FOC_PWM_FREQ);
cfg.notifier_freq = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
/* Print FOC device configuration */
foc_cfg_print(&cfg);
/* Configure FOC device */
ret = foc_dev_setcfg(motor->dev.fd, &cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setcfg %d!\n", ret);
goto errout;
}
errout: errout:
return ret; return ret;
} }
@ -308,13 +290,13 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
if (start == true) if (start == true)
{ {
/* Start device if VBUS data present */ /* Start motor if VBUS data present */
if (motor->mq.vbus > 0) if (motor->mq.vbus > 0)
{ {
/* Configure FOC device */ /* Configure motor controller */
PRINTF("Configure FOC device %d!\n", motor->envp->id); PRINTF("Configure motor %d!\n", motor->envp->id);
ret = foc_motor_configure(motor); ret = foc_motor_configure(motor);
if (ret < 0) if (ret < 0)
@ -323,16 +305,9 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
goto errout; goto errout;
} }
/* Start device */ /* Start/stop FOC dev request */
PRINTF("Start FOC device %d!\n", motor->envp->id); motor->startstop = true;
ret = foc_dev_start(motor->dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
goto errout;
}
} }
else else
{ {
@ -344,16 +319,9 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
} }
else else
{ {
/* Stop FOC device */ /* Start/stop FOC dev request */
PRINTF("Stop FOC device %d!\n", motor->envp->id); motor->startstop = true;
ret = foc_dev_stop(motor->dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
goto errout;
}
} }
errout: errout:
@ -576,7 +544,8 @@ errout:
* Name: foc_handler_run * Name: foc_handler_run
****************************************************************************/ ****************************************************************************/
static int foc_handler_run(FAR struct foc_motor_f32_s *motor) static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
FAR struct foc_device_s *dev)
{ {
struct foc_handler_input_f32_s input; struct foc_handler_input_f32_s input;
struct foc_handler_output_f32_s output; struct foc_handler_output_f32_s output;
@ -585,6 +554,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
int i = 0; int i = 0;
DEBUGASSERT(motor); DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* FOC device fault */ /* FOC device fault */
@ -606,7 +576,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
current[i] = (motor->iphase_adc * motor->dev.state.curr[i]); current[i] = (motor->iphase_adc * dev->state.curr[i]);
} }
/* Get input for FOC handler */ /* Get input for FOC handler */
@ -626,7 +596,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev.params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]); dev->params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
} }
/* Get FOC handler state */ /* Get FOC handler state */
@ -636,29 +606,19 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
return ret; return ret;
} }
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/**************************************************************************** /****************************************************************************
* Name: foc_dev_state_get * Name: foc_model_state_get
****************************************************************************/ ****************************************************************************/
static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor) static int foc_model_state_get(FAR struct foc_motor_f32_s *motor,
FAR struct foc_device_s *dev)
{ {
int ret = OK; int i = 0;
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
int i;
#endif
DEBUGASSERT(motor); DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* Get FOC state - blocking */
ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Get model state */ /* Get model state */
foc_model_state_f32(&motor->model, &motor->model_state); foc_model_state_f32(&motor->model, &motor->model_state);
@ -667,57 +627,12 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{ {
motor->dev.state.curr[i] = motor->model_state.curr_raw[i]; dev->state.curr[i] = motor->model_state.curr_raw[i];
}
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_params_set
****************************************************************************/
static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* Write FOC parameters */
ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
goto errout;
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_dev_state_handle
****************************************************************************/
static int foc_dev_state_handle(FAR struct foc_motor_f32_s *motor)
{
DEBUGASSERT(motor);
if (motor->dev.state.fault > 0)
{
PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true;
}
else
{
motor->fault = false;
} }
return OK; return OK;
} }
#endif
#ifdef FOC_STATE_PRINT_PRE #ifdef FOC_STATE_PRINT_PRE
@ -809,13 +724,15 @@ static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
motor->mq.app_state = handle->app_state; motor->mq.app_state = handle->app_state;
} }
/* Start/stop motor */ /* Start/stop controller */
if (motor->mq.start != handle->start) if (motor->mq.start != handle->start)
{ {
PRINTFV("Set start=%d for FOC driver %d!\n", PRINTFV("Set start=%d for FOC driver %d!\n",
handle->start, motor->envp->id); handle->start, motor->envp->id);
/* Start/stop motor controller */
ret = foc_motor_start(motor, handle->start); ret = foc_motor_start(motor, handle->start);
if (ret < 0) if (ret < 0)
{ {
@ -842,6 +759,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
{ {
struct foc_mq_s handle; struct foc_mq_s handle;
struct foc_motor_f32_s motor; struct foc_motor_f32_s motor;
struct foc_device_s dev;
int time = 0; int time = 0;
int ret = OK; int ret = OK;
@ -862,15 +780,19 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
/* Open FOC device as blocking */ /* Initialize FOC device as blocking */
ret = foc_device_open(&motor.dev, envp->id); ret = foc_device_init(&dev, envp->id);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_device_open failed %d!\n", ret); PRINTF("ERROR: foc_device_init failed %d!\n", ret);
goto errout; goto errout;
} }
/* Get PWM max duty */
motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max);
/* Initialize controller mode */ /* Initialize controller mode */
ret = foc_mode_init(&motor); ret = foc_mode_init(&motor);
@ -912,39 +834,56 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
goto errout; goto errout;
} }
if (motor.startstop == true)
{
/* Start or stop device */
PRINTF("Start FOC device %d state=%d!\n",
motor.envp->id, motor.mq.start);
ret = foc_device_start(&dev, motor.mq.start);
if (ret < 0)
{
PRINTFV("ERROR: foc_device_start failed %d!\n", ret);
goto errout;
}
motor.startstop = false;
}
/* Run control logic if controller started */ /* Run control logic if controller started */
if (motor.mq.start == true) if (motor.mq.start == true)
{ {
/* Get FOC device state */ /* Get FOC device state */
ret = foc_dev_state_get(&motor); ret = foc_dev_state_get(&dev);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret); PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout; goto errout;
} }
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* 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;
}
#endif
/* Handle controller state */ /* Handle controller state */
ret = foc_dev_state_handle(&motor); 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;
} }
if (motor.dev.state.fault > 0)
{
/* Clear fault state */
ret = foc_dev_clearfault(motor.dev.fd);
if (ret != OK)
{
goto errout;
}
}
/* Get motor state */ /* Get motor state */
ret = foc_motor_get(&motor); ret = foc_motor_get(&motor);
@ -965,7 +904,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Run FOC */ /* Run FOC */
ret = foc_handler_run(&motor); 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);
@ -991,7 +930,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Set FOC device parameters */ /* Set FOC device parameters */
ret = foc_dev_params_set(&motor); 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);
@ -1020,12 +959,12 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id); PRINTF("Stop FOC device %d!\n", envp->id);
/* Close FOC control device */ /* De-initialize FOC device */
ret = foc_device_close(&motor.dev); ret = foc_device_deinit(&dev);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret); PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret);
} }
PRINTF("foc_float_thr %d exit\n", envp->id); PRINTF("foc_float_thr %d exit\n", envp->id);