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 2021/10/31 17:13:56 UTC
[incubator-nuttx-apps] 04/05: examples/foc: move the common FOC dev
logic from threads to one place
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 22ed7da99c4f7a9b1aa69ef87af36b49ed663546
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 11:30:30 2021 +0100
examples/foc: move the common FOC dev logic from threads to one place
---
examples/foc/foc_device.c | 143 +++++++++++++++++++++++++++--
examples/foc/foc_device.h | 8 +-
examples/foc/foc_fixed16_thr.c | 198 ++++++++++++++---------------------------
examples/foc/foc_float_thr.c | 197 ++++++++++++++--------------------------
4 files changed, 279 insertions(+), 267 deletions(-)
diff --git a/examples/foc/foc_device.c b/examples/foc/foc_device.c
index 3e3570d..69429da 100644
--- a/examples/foc/foc_device.c
+++ b/examples/foc/foc_device.c
@@ -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];
- int ret = OK;
+ char devpath[32];
+ int ret = OK;
+ struct foc_cfg_s cfg;
DEBUGASSERT(dev);
@@ -69,15 +70,33 @@ int foc_device_open(FAR struct foc_device_s *dev, int id)
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:
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;
@@ -98,3 +117,115 @@ int foc_device_close(FAR struct foc_device_s *dev)
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;
+}
diff --git a/examples/foc/foc_device.h b/examples/foc/foc_device.h
index fc4d42b..f77c9be 100644
--- a/examples/foc/foc_device.h
+++ b/examples/foc/foc_device.h
@@ -51,7 +51,11 @@ struct foc_device_s
* Public Function Prototypes
****************************************************************************/
-int foc_device_open(FAR struct foc_device_s *dev, int id);
-int foc_device_close(FAR struct foc_device_s *dev);
+int foc_device_init(FAR struct foc_device_s *dev, int id);
+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 */
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 7009464..c86b928 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -64,8 +64,8 @@
struct foc_motor_b16_s
{
FAR struct foc_ctrl_env_s *envp; /* Thread env */
- struct foc_device_s dev; /* FOC device */
bool fault; /* Fault flag */
+ bool startstop; /* Start/stop request */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */
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 per; /* Controller period in seconds */
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 vdq_comp; /* DQ voltage compensation */
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
struct foc_model_pmsm_cfg_b16_s pmsm_cfg;
#endif
- struct foc_cfg_s cfg;
int ret = OK;
DEBUGASSERT(motor);
@@ -239,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* 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
/* 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);
#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:
return ret;
}
@@ -308,13 +290,13 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
if (start == true)
{
- /* Start device if VBUS data present */
+ /* Start motor if VBUS data present */
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);
if (ret < 0)
@@ -323,30 +305,16 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
goto errout;
}
- /* Start device */
-
- PRINTF("Start FOC device %d!\n", motor->envp->id);
+ /* Start/stop FOC dev request */
- ret = foc_dev_start(motor->dev.fd);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_start failed %d!\n", ret);
- goto errout;
- }
+ motor->startstop = true;
}
}
else
{
- /* Stop FOC device */
+ /* Start/stop FOC dev request */
- PRINTF("Stop FOC device %d!\n", motor->envp->id);
-
- ret = foc_dev_stop(motor->dev.fd);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_stop failed %d!\n", ret);
- goto errout;
- }
+ motor->startstop = true;
}
errout:
@@ -576,7 +544,8 @@ errout:
* 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_output_b16_s output;
@@ -585,6 +554,7 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
int i = 0;
DEBUGASSERT(motor);
+ DEBUGASSERT(dev);
/* 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)
{
- 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 */
@@ -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)
{
- 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 */
@@ -636,29 +606,19 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
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;
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- int i;
-#endif
+ int i = 0;
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 */
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)
{
- motor->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;
+ dev->state.curr[i] = motor->model_state.curr_raw[i];
}
return OK;
}
+#endif
#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;
}
- /* Start/stop motor */
+ /* Start/stop controller */
if (motor->mq.start != handle->start)
{
PRINTFV("Set start=%d for FOC driver %d!\n",
handle->start, motor->envp->id);
+ /* Start/stop motor controller */
+
ret = foc_motor_start(motor, handle->start);
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_motor_b16_s motor;
+ struct foc_device_s dev;
int time = 0;
int ret = OK;
@@ -859,15 +777,19 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
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)
{
- PRINTF("ERROR: foc_device_open failed %d!\n", ret);
+ PRINTF("ERROR: foc_device_init failed %d!\n", ret);
goto errout;
}
+ /* Get PWM max duty */
+
+ motor.pwm_duty_max = FOCDUTY_TO_FIXED16(dev.info.hw_cfg.pwm_max);
+
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -909,38 +831,54 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
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 */
if (motor.mq.start == true)
{
/* Get FOC device state */
- ret = foc_dev_state_get(&motor);
+ ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
- /* Handle controller state */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Get model state */
- ret = foc_dev_state_handle(&motor);
+ ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
- PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
+ PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
+#endif
- if (motor.dev.state.fault > 0)
+ /* Handle controller state */
+
+ ret = foc_dev_state_handle(&dev, &motor.fault);
+ if (ret < 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;
- }
+ PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
+ goto errout;
}
/* Get motor state */
@@ -963,7 +901,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Run FOC */
- ret = foc_handler_run(&motor);
+ ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
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 */
- ret = foc_dev_params_set(&motor);
+ ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_params_set failed %d!\n", ret);
@@ -1018,12 +956,12 @@ errout:
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)
{
- 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);
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 5f47652..fae5382 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -64,8 +64,8 @@
struct foc_motor_f32_s
{
FAR struct foc_ctrl_env_s *envp; /* Thread env */
- struct foc_device_s dev; /* FOC device */
bool fault; /* Fault flag */
+ bool startstop; /* Start/stop request */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */
float angle_ol; /* Phase angle open-loop */
@@ -80,6 +80,7 @@ struct foc_motor_f32_s
float dir; /* Motor's direction */
float per; /* Controller period in seconds */
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 vdq_comp; /* DQ voltage compensation */
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
struct foc_model_pmsm_cfg_f32_s pmsm_cfg;
#endif
- struct foc_cfg_s cfg;
int ret = OK;
DEBUGASSERT(motor);
@@ -239,7 +239,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* 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
/* 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);
#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:
return ret;
}
@@ -308,13 +290,13 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
if (start == true)
{
- /* Start device if VBUS data present */
+ /* Start motor if VBUS data present */
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);
if (ret < 0)
@@ -323,16 +305,9 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
goto errout;
}
- /* Start device */
-
- PRINTF("Start FOC device %d!\n", motor->envp->id);
+ /* Start/stop FOC dev request */
- ret = foc_dev_start(motor->dev.fd);
- if (ret < 0)
- {
- PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
- goto errout;
- }
+ motor->startstop = true;
}
else
{
@@ -344,16 +319,9 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
}
else
{
- /* Stop FOC device */
+ /* Start/stop FOC dev request */
- PRINTF("Stop FOC device %d!\n", motor->envp->id);
-
- ret = foc_dev_stop(motor->dev.fd);
- if (ret < 0)
- {
- PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
- goto errout;
- }
+ motor->startstop = true;
}
errout:
@@ -576,7 +544,8 @@ errout:
* 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_output_f32_s output;
@@ -585,6 +554,7 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
int i = 0;
DEBUGASSERT(motor);
+ DEBUGASSERT(dev);
/* 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)
{
- current[i] = (motor->iphase_adc * motor->dev.state.curr[i]);
+ current[i] = (motor->iphase_adc * dev->state.curr[i]);
}
/* 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)
{
- 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 */
@@ -636,29 +606,19 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
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;
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- int i;
-#endif
+ int i = 0;
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 */
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)
{
- motor->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;
+ dev->state.curr[i] = motor->model_state.curr_raw[i];
}
return OK;
}
+#endif
#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;
}
- /* Start/stop motor */
+ /* Start/stop controller */
if (motor->mq.start != handle->start)
{
PRINTFV("Set start=%d for FOC driver %d!\n",
handle->start, motor->envp->id);
+ /* Start/stop motor controller */
+
ret = foc_motor_start(motor, handle->start);
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_motor_f32_s motor;
+ struct foc_device_s dev;
int time = 0;
int ret = OK;
@@ -862,15 +780,19 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
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)
{
- PRINTF("ERROR: foc_device_open failed %d!\n", ret);
+ PRINTF("ERROR: foc_device_init failed %d!\n", ret);
goto errout;
}
+ /* Get PWM max duty */
+
+ motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max);
+
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -912,37 +834,54 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
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 */
if (motor.mq.start == true)
{
/* Get FOC device state */
- ret = foc_dev_state_get(&motor);
+ ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
- /* Handle controller state */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Get model state */
- ret = foc_dev_state_handle(&motor);
+ ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
- PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
+ PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
+#endif
- if (motor.dev.state.fault > 0)
- {
- /* Clear fault state */
+ /* Handle controller state */
- ret = foc_dev_clearfault(motor.dev.fd);
- if (ret != OK)
- {
- 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 */
@@ -965,7 +904,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Run FOC */
- ret = foc_handler_run(&motor);
+ ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
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 */
- ret = foc_dev_params_set(&motor);
+ ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
@@ -1020,12 +959,12 @@ errout:
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)
{
- 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);