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:52 UTC
[incubator-nuttx-apps] branch master updated (3ba18e5 -> f9cec1c)
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git.
from 3ba18e5 testing/ostest: Fix the compiler warning
new 2d034ed examples/foc: separate motor control logic from motor state logic
new 33b34f8 examples/foc: open FOC device in FOC threads
new e0ef3ce examples/foc: move FOC device data to foc_device_s
new 22ed7da examples/foc: move the common FOC dev logic from threads to one place
new f9cec1c examples/foc: move the motor controller code to separate files
The 5 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails. The revisions
listed as "add" were already present in the repository and have only
been added to this reference.
Summary of changes:
examples/foc/Makefile | 4 +-
examples/foc/foc_device.c | 160 ++++++++-
examples/foc/foc_device.h | 15 +-
examples/foc/foc_fixed16_thr.c | 725 +++++-----------------------------------
examples/foc/foc_float_thr.c | 726 +++++------------------------------------
examples/foc/foc_main.c | 17 -
examples/foc/foc_motor_b16.c | 541 ++++++++++++++++++++++++++++++
examples/foc/foc_motor_b16.h | 99 ++++++
examples/foc/foc_motor_f32.c | 543 ++++++++++++++++++++++++++++++
examples/foc/foc_motor_f32.h | 100 ++++++
examples/foc/foc_thr.h | 1 -
11 files changed, 1597 insertions(+), 1334 deletions(-)
create mode 100644 examples/foc/foc_motor_b16.c
create mode 100644 examples/foc/foc_motor_b16.h
create mode 100644 examples/foc/foc_motor_f32.c
create mode 100644 examples/foc/foc_motor_f32.h
[incubator-nuttx-apps] 03/05: examples/foc: move FOC device data to
foc_device_s
Posted by xi...@apache.org.
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 e0ef3cecb9bd64558e603a83dd47718d51b662b5
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 10:54:22 2021 +0100
examples/foc: move FOC device data to foc_device_s
---
examples/foc/foc_device.c | 17 +++++++++++++++
examples/foc/foc_device.h | 7 ++++++-
examples/foc/foc_fixed16_thr.c | 45 +++++++++++-----------------------------
examples/foc/foc_float_thr.c | 47 ++++++++++++------------------------------
4 files changed, 48 insertions(+), 68 deletions(-)
diff --git a/examples/foc/foc_device.c b/examples/foc/foc_device.c
index 605ec67..3e3570d 100644
--- a/examples/foc/foc_device.c
+++ b/examples/foc/foc_device.c
@@ -60,6 +60,15 @@ int foc_device_open(FAR struct foc_device_s *dev, int id)
goto errout;
}
+ /* Get device info */
+
+ ret = foc_dev_getinfo(dev->fd, &dev->info);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
+ goto errout;
+ }
+
errout:
return ret;
}
@@ -74,6 +83,14 @@ int foc_device_close(FAR struct foc_device_s *dev)
DEBUGASSERT(dev);
+ /* Stop FOC device */
+
+ ret = foc_dev_stop(dev->fd);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_stop %d!\n", ret);
+ }
+
if (dev->fd > 0)
{
close(dev->fd);
diff --git a/examples/foc/foc_device.h b/examples/foc/foc_device.h
index be5be92..fc4d42b 100644
--- a/examples/foc/foc_device.h
+++ b/examples/foc/foc_device.h
@@ -27,6 +27,8 @@
#include <nuttx/config.h>
+#include "industry/foc/foc_utils.h"
+
/****************************************************************************
* Public Type Definition
****************************************************************************/
@@ -35,7 +37,10 @@
struct foc_device_s
{
- int fd; /* FOC device */
+ int fd; /* FOC device */
+ struct foc_info_s info; /* FOC dev info */
+ struct foc_state_s state; /* FOC dev state */
+ struct foc_params_s params; /* FOC dev params */
};
/****************************************************************************
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 44fb1e6..7009464 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -84,10 +84,7 @@ struct foc_motor_b16_s
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
foc_handler_b16_t handler; /* FOC controller */
struct foc_mq_s mq; /* MQ data */
- struct foc_info_s info; /* Device info */
struct foc_state_b16_s foc_state; /* FOC controller sate */
- struct foc_state_s dev_state; /* FOC dev state */
- struct foc_params_s dev_params; /* FOC dev params */
struct foc_ramp_b16_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_b16_s model; /* Model handler */
@@ -139,7 +136,6 @@ static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
#endif
-errout:
return ret;
}
@@ -243,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->info.hw_cfg.pwm_max);
+ mod_cfg.pwm_duty_max = FOCDUTY_TO_FIXED16(motor->dev.info.hw_cfg.pwm_max);
#endif
/* Configure FOC handler */
@@ -610,7 +606,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, motor->dev.state.curr[i]);
}
/* Get input for FOC handler */
@@ -630,7 +626,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]);
+ motor->dev.params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
}
/* Get FOC handler state */
@@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
/* Get FOC state - blocking */
- ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state);
+ ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret);
@@ -671,7 +667,7 @@ 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];
+ motor->dev.state.curr[i] = motor->model_state.curr_raw[i];
}
#endif
@@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor)
/* Write FOC parameters */
- ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params);
+ ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret);
@@ -703,16 +699,16 @@ errout:
}
/****************************************************************************
- * Name: foc_state_handle
+ * Name: foc_dev_state_handle
****************************************************************************/
-static int foc_state_handle(FAR struct foc_motor_b16_s *motor)
+static int foc_dev_state_handle(FAR struct foc_motor_b16_s *motor)
{
DEBUGASSERT(motor);
- if (motor->dev_state.fault > 0)
+ if (motor->dev.state.fault > 0)
{
- PRINTF("FAULT = %d\n", motor->dev_state.fault);
+ PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true;
}
else
@@ -872,15 +868,6 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout;
}
- /* Get device info */
-
- ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
- goto errout;
- }
-
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -937,14 +924,14 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
/* Handle controller state */
- ret = foc_state_handle(&motor);
+ ret = foc_dev_state_handle(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
- if (motor.dev_state.fault > 0)
+ if (motor.dev.state.fault > 0)
{
/* Clear fault state */
@@ -1031,14 +1018,6 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id);
- /* Stop FOC device */
-
- ret = foc_dev_stop(motor.dev.fd);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_stop %d!\n", ret);
- }
-
/* Close FOC control device */
ret = foc_device_close(&motor.dev);
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 1386ae8..5f47652 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -84,10 +84,7 @@ struct foc_motor_f32_s
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
foc_handler_f32_t handler; /* FOC controller */
struct foc_mq_s mq; /* MQ data */
- struct foc_info_s info; /* Device info */
struct foc_state_f32_s foc_state; /* FOC controller sate */
- struct foc_state_s dev_state; /* FOC dev state */
- struct foc_params_s dev_params; /* FOC dev params */
struct foc_ramp_f32_s ramp; /* Velocity ramp data */
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_f32_s model; /* Model handler */
@@ -139,7 +136,6 @@ static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
#endif
-errout:
return ret;
}
@@ -243,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->info.hw_cfg.pwm_max);
+ mod_cfg.pwm_duty_max = FOCDUTY_TO_FLOAT(motor->dev.info.hw_cfg.pwm_max);
#endif
/* Configure FOC handler */
@@ -610,7 +606,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 * motor->dev.state.curr[i]);
}
/* Get input for FOC handler */
@@ -630,7 +626,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]);
+ motor->dev.params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
}
/* Get FOC handler state */
@@ -655,7 +651,7 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
/* Get FOC state - blocking */
- ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state);
+ ret = foc_dev_getstate(motor->dev.fd, &motor->dev.state);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
@@ -671,7 +667,7 @@ 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];
+ motor->dev.state.curr[i] = motor->model_state.curr_raw[i];
}
#endif
@@ -691,7 +687,7 @@ static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor)
/* Write FOC parameters */
- ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params);
+ ret = foc_dev_setparams(motor->dev.fd, &motor->dev.params);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
@@ -703,16 +699,16 @@ errout:
}
/****************************************************************************
- * Name: foc_state_handle
+ * Name: foc_dev_state_handle
****************************************************************************/
-static int foc_state_handle(FAR struct foc_motor_f32_s *motor)
+static int foc_dev_state_handle(FAR struct foc_motor_f32_s *motor)
{
DEBUGASSERT(motor);
- if (motor->dev_state.fault > 0)
+ if (motor->dev.state.fault > 0)
{
- PRINTF("FAULT = %d\n", motor->dev_state.fault);
+ PRINTF("FAULT = %d\n", motor->dev.state.fault);
motor->fault = true;
}
else
@@ -875,15 +871,6 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
goto errout;
}
- /* Get device info */
-
- ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
- goto errout;
- }
-
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -940,14 +927,14 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
/* Handle controller state */
- ret = foc_state_handle(&motor);
+ ret = foc_dev_state_handle(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_state_handle failed %d!\n", ret);
+ PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
- if (motor.dev_state.fault > 0)
+ if (motor.dev.state.fault > 0)
{
/* Clear fault state */
@@ -1033,14 +1020,6 @@ errout:
PRINTF("Stop FOC device %d!\n", envp->id);
- /* Stop FOC device */
-
- ret = foc_dev_stop(motor.dev.fd);
- if (ret < 0)
- {
- PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
- }
-
/* Close FOC control device */
ret = foc_device_close(&motor.dev);
[incubator-nuttx-apps] 04/05: examples/foc: move the common FOC dev
logic from threads to one place
Posted by xi...@apache.org.
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);
[incubator-nuttx-apps] 05/05: examples/foc: move the motor
controller code to separate files
Posted by xi...@apache.org.
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 f9cec1c7706cab52b1baf9f3d703f298ce2322d5
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 16:07:22 2021 +0100
examples/foc: move the motor controller code to separate files
---
examples/foc/Makefile | 4 +-
examples/foc/foc_fixed16_thr.c | 538 +---------------
examples/foc/foc_float_thr.c | 540 +---------------
.../foc/{foc_fixed16_thr.c => foc_motor_b16.c} | 677 ++++----------------
examples/foc/foc_motor_b16.h | 99 +++
examples/foc/{foc_float_thr.c => foc_motor_f32.c} | 690 ++++-----------------
examples/foc/foc_motor_f32.h | 100 +++
7 files changed, 457 insertions(+), 2191 deletions(-)
diff --git a/examples/foc/Makefile b/examples/foc/Makefile
index 96e9e73..1fb157b 100644
--- a/examples/foc/Makefile
+++ b/examples/foc/Makefile
@@ -41,13 +41,13 @@ endif
# fixed16 support
ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y)
- CSRCS += foc_fixed16_thr.c
+ CSRCS += foc_fixed16_thr.c foc_motor_b16.c
endif
# float32 support
ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
- CSRCS += foc_float_thr.c
+ CSRCS += foc_float_thr.c foc_motor_f32.c
endif
include $(APPDIR)/Application.mk
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index c86b928..ce816bb 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -30,22 +30,12 @@
#include <dspb16.h>
-#include "foc_mq.h"
-#include "foc_thr.h"
#include "foc_cfg.h"
-#include "foc_adc.h"
-
#include "foc_debug.h"
+#include "foc_motor_b16.h"
#include "industry/foc/foc_utils.h"
#include "industry/foc/foc_common.h"
-#include "industry/foc/fixed16/foc_handler.h"
-#include "industry/foc/fixed16/foc_ramp.h"
-#include "industry/foc/fixed16/foc_angle.h"
-#include "industry/foc/fixed16/foc_velocity.h"
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
-# include "industry/foc/fixed16/foc_model.h"
-#endif
/****************************************************************************
* Pre-processor Definitions
@@ -59,88 +49,11 @@
* Private Type Definition
****************************************************************************/
-/* FOC motor data */
-
-struct foc_motor_b16_s
-{
- FAR struct foc_ctrl_env_s *envp; /* Thread env */
- 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 */
- foc_angle_b16_t openloop; /* Open-loop angle handler */
-#endif
- int foc_mode; /* FOC mode */
- b16_t vbus; /* Power bus voltage */
- b16_t angle_now; /* Phase angle now */
- b16_t vel_set; /* Velocity setting now */
- b16_t vel_now; /* Velocity now */
- b16_t vel_des; /* Velocity destination */
- 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 */
- struct foc_mq_s mq; /* MQ data */
- struct foc_state_b16_s foc_state; /* FOC controller sate */
- struct foc_ramp_b16_s ramp; /* Velocity ramp data */
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_b16_s model; /* Model handler */
- struct foc_model_state_b16_s model_state; /* PMSM model state */
-#endif
-};
-
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
- * Name: foc_motor_init
- ****************************************************************************/
-
-static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_ctrl_env_s *envp)
-{
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- struct foc_openloop_cfg_b16_s ol_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(envp);
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_b16_s));
-
- /* Connect envp with motor handler */
-
- motor->envp = envp;
-
- /* Initialize motor data */
-
- motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
- motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Initialize open-loop angle handler */
-
- foc_angle_init_b16(&motor->openloop,
- &g_foc_angle_ol_b16);
-
- /* Configure open-loop angle handler */
-
- ol_cfg.per = motor->per;
- foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
-#endif
-
- return ret;
-}
-
-/****************************************************************************
* Name: foc_mode_init
****************************************************************************/
@@ -185,362 +98,6 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_configure
- ****************************************************************************/
-
-static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
-{
-#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
- struct foc_initdata_b16_s ctrl_cfg;
-#endif
-#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
- struct foc_mod_cfg_b16_s mod_cfg;
-#endif
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_pmsm_cfg_b16_s pmsm_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Initialize velocity ramp */
-
- ret = foc_ramp_init_b16(&motor->ramp,
- motor->per,
- ftob16(RAMP_CFG_THR),
- ftob16(RAMP_CFG_ACC),
- ftob16(RAMP_CFG_ACC));
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
- goto errout;
- }
-
- /* Initialize FOC handler */
-
- ret = foc_handler_init_b16(&motor->handler,
- &g_foc_control_pi_b16,
- &g_foc_mod_svm3_b16);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_init failed %d\n", ret);
- goto errout;
- }
-
-#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
- /* Get PI controller configuration */
-
- ctrl_cfg.id_kp = ftob16(motor->envp->pi_kp / 1000.0f);
- ctrl_cfg.id_ki = ftob16(motor->envp->pi_ki / 1000.0f);
- ctrl_cfg.iq_kp = ftob16(motor->envp->pi_kp / 1000.0f);
- ctrl_cfg.iq_ki = ftob16(motor->envp->pi_ki / 1000.0f);
-#endif
-
-#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
- /* Get SVM3 modulation configuration */
-
- mod_cfg.pwm_duty_max = motor->pwm_duty_max;
-#endif
-
- /* Configure FOC handler */
-
- foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Initialize PMSM model */
-
- ret = foc_model_init_b16(&motor->model,
- &g_foc_model_pmsm_ops_b16);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_init failed %d\n", ret);
- goto errout;
- }
-
- /* Get PMSM model configuration */
-
- pmsm_cfg.poles = FOC_MODEL_POLES;
- pmsm_cfg.res = ftob16(FOC_MODEL_RES);
- pmsm_cfg.ind = ftob16(FOC_MODEL_IND);
- pmsm_cfg.iner = ftob16(FOC_MODEL_INER);
- pmsm_cfg.flux_link = ftob16(FOC_MODEL_FLUX);
- pmsm_cfg.ind_d = ftob16(FOC_MODEL_INDD);
- pmsm_cfg.ind_q = ftob16(FOC_MODEL_INDQ);
- pmsm_cfg.per = motor->per;
- pmsm_cfg.iphase_adc = motor->iphase_adc;
-
- /* Configure PMSM model */
-
- foc_model_cfg_b16(&motor->model, &pmsm_cfg);
-#endif
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_start
- ****************************************************************************/
-
-static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- if (start == true)
- {
- /* Start motor if VBUS data present */
-
- if (motor->mq.vbus > 0)
- {
- /* Configure motor controller */
-
- PRINTF("Configure motor %d!\n", motor->envp->id);
-
- ret = foc_motor_configure(motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
- goto errout;
- }
-
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
- }
- else
- {
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_deinit
- ****************************************************************************/
-
-static int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Deinitialize PMSM model */
-
- ret = foc_model_deinit_b16(&motor->model);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
- goto errout;
- }
-#endif
-
- /* Deinitialize FOC handler */
-
- ret = foc_handler_deinit_b16(&motor->handler);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
- goto errout;
- }
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_b16_s));
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_vbus
- ****************************************************************************/
-
-static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus)
-{
- DEBUGASSERT(motor);
-
- /* Update motor VBUS */
-
- motor->vbus = b16muli(vbus, ftob16(VBUS_ADC_SCALE));
-
- return OK;
-}
-
-/****************************************************************************
- * Name: foc_motor_vel
- ****************************************************************************/
-
-static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel)
-{
- b16_t tmp1 = 0;
- b16_t tmp2 = 0;
-
- DEBUGASSERT(motor);
-
- /* Update motor velocity destination
- * NOTE: velmax may not fit in b16_t so we can't use b16idiv()
- */
-
- tmp1 = itob16(motor->envp->velmax / 1000);
- tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1);
-
- motor->vel_des = b16muli(tmp2, vel);
-
- return OK;
-}
-
-/****************************************************************************
- * Name: foc_motor_state
- ****************************************************************************/
-
-static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Get open-loop currents
- * NOTE: Id always set to 0
- */
-
- motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000);
- motor->dq_ref.d = 0;
-
- /* Update motor state */
-
- switch (state)
- {
- case FOC_EXAMPLE_STATE_FREE:
- {
- motor->vel_set = 0;
- motor->dir = DIR_NONE_B16;
-
- /* Force DQ vector to zero */
-
- motor->dq_ref.q = 0;
- motor->dq_ref.d = 0;
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_STOP:
- {
- motor->vel_set = 0;
- motor->dir = DIR_NONE_B16;
-
- /* DQ vector not zero - active brake */
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_CW:
- {
- motor->vel_set = 0;
- motor->dir = DIR_CW_B16;
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_CCW:
- {
- motor->vel_set = 0;
- motor->dir = DIR_CCW_B16;
-
- break;
- }
-
- default:
- {
- ret = -EINVAL;
- goto errout;
- }
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_get
- ****************************************************************************/
-
-static int foc_motor_get(FAR struct foc_motor_b16_s *motor)
-{
- struct foc_angle_in_b16_s ain;
- struct foc_angle_out_b16_s aout;
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Update open-loop angle handler */
-
- ain.vel = motor->vel_set;
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_b16(&motor->openloop, &ain, &aout);
-
- /* Store open-loop angle */
-
- motor->angle_ol = aout.angle;
-
- /* Get phase angle now */
-
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
- }
- else
-#endif
- {
- /* TODO: get phase angle from observer or sensor */
-
- ASSERT(0);
- }
-
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_control
- ****************************************************************************/
-
-static int foc_motor_control(FAR struct foc_motor_b16_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
-
- motor->vel_now = motor->vel_set;
-
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_handler_run
****************************************************************************/
@@ -652,99 +209,6 @@ static int foc_state_print(FAR struct foc_motor_b16_s *motor)
#endif
/****************************************************************************
- * Name: foc_motor_handle
- ****************************************************************************/
-
-static int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_mq_s *handle)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(handle);
-
- /* Terminate */
-
- if (handle->quit == true)
- {
- motor->mq.quit = true;
- }
-
- /* Update motor VBUS */
-
- if (motor->mq.vbus != handle->vbus)
- {
- PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n",
- handle->vbus, motor->envp->id);
-
- ret = foc_motor_vbus(motor, handle->vbus);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.vbus = handle->vbus;
- }
-
- /* Update motor velocity destination */
-
- if (motor->mq.setpoint != handle->setpoint)
- {
- PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
- handle->setpoint, motor->envp->id);
-
- ret = foc_motor_vel(motor, handle->setpoint);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.setpoint = handle->setpoint;
- }
-
- /* Update motor state */
-
- if (motor->mq.app_state != handle->app_state)
- {
- PRINTFV("Set app_state=%d for FOC driver %d!\n",
- handle->app_state, motor->envp->id);
-
- ret = foc_motor_state(motor, handle->app_state);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_state failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.app_state = handle->app_state;
- }
-
- /* 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)
- {
- PRINTF("ERROR: foc_motor_start failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.start = handle->start;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Public Functions
****************************************************************************/
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index fae5382..0db14be 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -30,22 +30,12 @@
#include <dsp.h>
-#include "foc_mq.h"
-#include "foc_thr.h"
#include "foc_cfg.h"
-#include "foc_adc.h"
-
#include "foc_debug.h"
+#include "foc_motor_f32.h"
#include "industry/foc/foc_utils.h"
#include "industry/foc/foc_common.h"
-#include "industry/foc/float/foc_handler.h"
-#include "industry/foc/float/foc_ramp.h"
-#include "industry/foc/float/foc_angle.h"
-#include "industry/foc/float/foc_velocity.h"
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
-# include "industry/foc/float/foc_model.h"
-#endif
/****************************************************************************
* Pre-processor Definitions
@@ -59,88 +49,11 @@
* Private Type Definition
****************************************************************************/
-/* FOC motor data */
-
-struct foc_motor_f32_s
-{
- FAR struct foc_ctrl_env_s *envp; /* Thread env */
- 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 */
- foc_angle_f32_t openloop; /* Open-loop angle handler */
-#endif
- int foc_mode; /* FOC mode */
- float vbus; /* Power bus voltage */
- float angle_now; /* Phase angle now */
- float vel_set; /* Velocity setting now */
- float vel_now; /* Velocity now */
- float vel_des; /* Velocity destination */
- 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 */
- struct foc_mq_s mq; /* MQ data */
- struct foc_state_f32_s foc_state; /* FOC controller sate */
- struct foc_ramp_f32_s ramp; /* Velocity ramp data */
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_f32_s model; /* Model handler */
- struct foc_model_state_f32_s model_state; /* PMSM model state */
-#endif
-};
-
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
- * Name: foc_motor_init
- ****************************************************************************/
-
-static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_ctrl_env_s *envp)
-{
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- struct foc_openloop_cfg_f32_s ol_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(envp);
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_f32_s));
-
- /* Connect envp with motor handler */
-
- motor->envp = envp;
-
- /* Initialize motor data */
-
- motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
- motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Initialize open-loop angle handler */
-
- foc_angle_init_f32(&motor->openloop,
- &g_foc_angle_ol_f32);
-
- /* Configure open-loop angle handler */
-
- ol_cfg.per = motor->per;
- foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
-#endif
-
- return ret;
-}
-
-/****************************************************************************
* Name: foc_mode_init
****************************************************************************/
@@ -185,362 +98,6 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_configure
- ****************************************************************************/
-
-static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
-{
-#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
- struct foc_initdata_f32_s ctrl_cfg;
-#endif
-#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
- struct foc_mod_cfg_f32_s mod_cfg;
-#endif
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_pmsm_cfg_f32_s pmsm_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Initialize velocity ramp */
-
- ret = foc_ramp_init_f32(&motor->ramp,
- motor->per,
- RAMP_CFG_THR,
- RAMP_CFG_ACC,
- RAMP_CFG_ACC);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
- goto errout;
- }
-
- /* Initialize FOC handler */
-
- ret = foc_handler_init_f32(&motor->handler,
- &g_foc_control_pi_f32,
- &g_foc_mod_svm3_f32);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_init failed %d\n", ret);
- goto errout;
- }
-
-#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
- /* Get PI controller configuration */
-
- ctrl_cfg.id_kp = (motor->envp->pi_kp / 1000.0f);
- ctrl_cfg.id_ki = (motor->envp->pi_ki / 1000.0f);
- ctrl_cfg.iq_kp = (motor->envp->pi_kp / 1000.0f);
- ctrl_cfg.iq_ki = (motor->envp->pi_ki / 1000.0f);
-#endif
-
-#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
- /* Get SVM3 modulation configuration */
-
- mod_cfg.pwm_duty_max = motor->pwm_duty_max;
-#endif
-
- /* Configure FOC handler */
-
- foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Initialize PMSM model */
-
- ret = foc_model_init_f32(&motor->model,
- &g_foc_model_pmsm_ops_f32);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_init failed %d\n", ret);
- goto errout;
- }
-
- /* Get PMSM model configuration */
-
- pmsm_cfg.poles = FOC_MODEL_POLES;
- pmsm_cfg.res = FOC_MODEL_RES;
- pmsm_cfg.ind = FOC_MODEL_IND;
- pmsm_cfg.iner = FOC_MODEL_INER;
- pmsm_cfg.flux_link = FOC_MODEL_FLUX;
- pmsm_cfg.ind_d = FOC_MODEL_INDD;
- pmsm_cfg.ind_q = FOC_MODEL_INDQ;
- pmsm_cfg.per = motor->per;
- pmsm_cfg.iphase_adc = motor->iphase_adc;
-
- /* Configure PMSM model */
-
- foc_model_cfg_f32(&motor->model, &pmsm_cfg);
-#endif
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_start
- ****************************************************************************/
-
-static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- if (start == true)
- {
- /* Start motor if VBUS data present */
-
- if (motor->mq.vbus > 0)
- {
- /* Configure motor controller */
-
- PRINTF("Configure motor %d!\n", motor->envp->id);
-
- ret = foc_motor_configure(motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
- goto errout;
- }
-
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
- else
- {
- /* Return error if no VBUS data */
-
- PRINTF("ERROR: start request without VBUS!\n");
- goto errout;
- }
- }
- else
- {
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_deinit
- ****************************************************************************/
-
-static int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Deinitialize PMSM model */
-
- ret = foc_model_deinit_f32(&motor->model);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
- goto errout;
- }
-#endif
-
- /* Deinitialize FOC handler */
-
- ret = foc_handler_deinit_f32(&motor->handler);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
- goto errout;
- }
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_f32_s));
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_vbus
- ****************************************************************************/
-
-static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus)
-{
- DEBUGASSERT(motor);
-
- /* Update motor VBUS */
-
- motor->vbus = (vbus * VBUS_ADC_SCALE);
-
- return OK;
-}
-
-/****************************************************************************
- * Name: foc_motor_vel
- ****************************************************************************/
-
-static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
-{
- DEBUGASSERT(motor);
-
- /* Update motor velocity destination */
-
- motor->vel_des = (vel * SETPOINT_ADC_SCALE *
- motor->envp->velmax / 1000.0f);
-
- return OK;
-}
-
-/****************************************************************************
- * Name: foc_motor_state
- ****************************************************************************/
-
-static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Get open-loop currents
- * NOTE: Id always set to 0
- */
-
- motor->dq_ref.q = (motor->envp->qparam / 1000.0f);
- motor->dq_ref.d = 0.0f;
-
- /* Update motor state */
-
- switch (state)
- {
- case FOC_EXAMPLE_STATE_FREE:
- {
- motor->vel_set = 0.0f;
- motor->dir = DIR_NONE;
-
- /* Force DQ vector to zero */
-
- motor->dq_ref.q = 0.0f;
- motor->dq_ref.d = 0.0f;
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_STOP:
- {
- motor->vel_set = 0.0f;
- motor->dir = DIR_NONE;
-
- /* DQ vector not zero - active brake */
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_CW:
- {
- motor->vel_set = 0.0f;
- motor->dir = DIR_CW;
-
- break;
- }
-
- case FOC_EXAMPLE_STATE_CCW:
- {
- motor->vel_set = 0.0f;
- motor->dir = DIR_CCW;
-
- break;
- }
-
- default:
- {
- ret = -EINVAL;
- goto errout;
- }
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_get
- ****************************************************************************/
-
-static int foc_motor_get(FAR struct foc_motor_f32_s *motor)
-{
- struct foc_angle_in_f32_s ain;
- struct foc_angle_out_f32_s aout;
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* Update open-loop angle handler */
-
- ain.vel = motor->vel_set;
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_f32(&motor->openloop, &ain, &aout);
-
- /* Store open-loop angle */
-
- motor->angle_ol = aout.angle;
-
- /* Get phase angle now */
-
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
- }
- else
-#endif
- {
- /* TODO: get phase angle from observer or sensor */
-
- ASSERT(0);
- }
-
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_control
- ****************************************************************************/
-
-static int foc_motor_control(FAR struct foc_motor_f32_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
-
- motor->vel_now = motor->vel_set;
-
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_handler_run
****************************************************************************/
@@ -653,101 +210,6 @@ static int foc_state_print(FAR struct foc_motor_f32_s *motor)
#endif
/****************************************************************************
- * Name: foc_motor_handle
- ****************************************************************************/
-
-static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_mq_s *handle)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(handle);
-
- /* Terminate */
-
- if (handle->quit == true)
- {
- motor->mq.quit = true;
- }
-
- /* Update motor VBUS */
-
- if (motor->mq.vbus != handle->vbus)
- {
- PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n",
- handle->vbus, motor->envp->id);
-
- ret = foc_motor_vbus(motor, handle->vbus);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.vbus = handle->vbus;
- }
-
- /* Update motor velocity destination
- * NOTE: only velocity control supported for now
- */
-
- if (motor->mq.setpoint != handle->setpoint)
- {
- PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
- handle->setpoint, motor->envp->id);
-
- ret = foc_motor_vel(motor, handle->setpoint);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.setpoint = handle->setpoint;
- }
-
- /* Update motor state */
-
- if (motor->mq.app_state != handle->app_state)
- {
- PRINTFV("Set app_state=%d for FOC driver %d!\n",
- handle->app_state, motor->envp->id);
-
- ret = foc_motor_state(motor, handle->app_state);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_state failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.app_state = handle->app_state;
- }
-
- /* 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)
- {
- PRINTF("ERROR: foc_motor_start failed %d!\n", ret);
- goto errout;
- }
-
- motor->mq.start = handle->start;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Public Functions
****************************************************************************/
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_motor_b16.c
similarity index 53%
copy from examples/foc/foc_fixed16_thr.c
copy to examples/foc/foc_motor_b16.c
index c86b928..ab543f2 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_motor_b16.c
@@ -1,5 +1,5 @@
/****************************************************************************
- * apps/examples/foc/foc_fixed16_thr.c
+ * apps/examples/foc/foc_motor_b16.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
@@ -24,167 +24,27 @@
#include <nuttx/config.h>
-#include <stdio.h>
-#include <fcntl.h>
#include <assert.h>
-#include <dspb16.h>
+#include "foc_motor_b16.h"
-#include "foc_mq.h"
-#include "foc_thr.h"
#include "foc_cfg.h"
#include "foc_adc.h"
-
#include "foc_debug.h"
-#include "industry/foc/foc_utils.h"
-#include "industry/foc/foc_common.h"
-#include "industry/foc/fixed16/foc_handler.h"
-#include "industry/foc/fixed16/foc_ramp.h"
-#include "industry/foc/fixed16/foc_angle.h"
-#include "industry/foc/fixed16/foc_velocity.h"
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
-# include "industry/foc/fixed16/foc_model.h"
-#endif
-
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
-#ifndef CONFIG_INDUSTRY_FOC_FIXED16
-# error
-#endif
-
/****************************************************************************
* Private Type Definition
****************************************************************************/
-/* FOC motor data */
-
-struct foc_motor_b16_s
-{
- FAR struct foc_ctrl_env_s *envp; /* Thread env */
- 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 */
- foc_angle_b16_t openloop; /* Open-loop angle handler */
-#endif
- int foc_mode; /* FOC mode */
- b16_t vbus; /* Power bus voltage */
- b16_t angle_now; /* Phase angle now */
- b16_t vel_set; /* Velocity setting now */
- b16_t vel_now; /* Velocity now */
- b16_t vel_des; /* Velocity destination */
- 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 */
- struct foc_mq_s mq; /* MQ data */
- struct foc_state_b16_s foc_state; /* FOC controller sate */
- struct foc_ramp_b16_s ramp; /* Velocity ramp data */
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_b16_s model; /* Model handler */
- struct foc_model_state_b16_s model_state; /* PMSM model state */
-#endif
-};
-
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
- * Name: foc_motor_init
- ****************************************************************************/
-
-static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_ctrl_env_s *envp)
-{
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- struct foc_openloop_cfg_b16_s ol_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(envp);
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_b16_s));
-
- /* Connect envp with motor handler */
-
- motor->envp = envp;
-
- /* Initialize motor data */
-
- motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
- motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Initialize open-loop angle handler */
-
- foc_angle_init_b16(&motor->openloop,
- &g_foc_angle_ol_b16);
-
- /* Configure open-loop angle handler */
-
- ol_cfg.per = motor->per;
- foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
-#endif
-
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_mode_init
- ****************************************************************************/
-
-static int foc_mode_init(FAR struct foc_motor_b16_s *motor)
-{
- int ret = OK;
-
- switch (motor->envp->mode)
- {
- case FOC_OPMODE_IDLE:
- {
- motor->foc_mode = FOC_HANDLER_MODE_IDLE;
- break;
- }
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- case FOC_OPMODE_OL_V_VEL:
- {
- motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
- motor->openloop_now = true;
- break;
- }
-
- case FOC_OPMODE_OL_C_VEL:
- {
- motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
- motor->openloop_now = true;
- break;
- }
-#endif
-
- default:
- {
- PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
- ret = -EINVAL;
- goto errout;
- }
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_motor_configure
****************************************************************************/
@@ -279,87 +139,6 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_start
- ****************************************************************************/
-
-static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- if (start == true)
- {
- /* Start motor if VBUS data present */
-
- if (motor->mq.vbus > 0)
- {
- /* Configure motor controller */
-
- PRINTF("Configure motor %d!\n", motor->envp->id);
-
- ret = foc_motor_configure(motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
- goto errout;
- }
-
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
- }
- else
- {
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_deinit
- ****************************************************************************/
-
-static int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Deinitialize PMSM model */
-
- ret = foc_model_deinit_b16(&motor->model);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
- goto errout;
- }
-#endif
-
- /* Deinitialize FOC handler */
-
- ret = foc_handler_deinit_b16(&motor->handler);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
- goto errout;
- }
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_b16_s));
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_motor_vbus
****************************************************************************/
@@ -469,194 +248,211 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_get
+ * Name: foc_motor_start
****************************************************************************/
-static int foc_motor_get(FAR struct foc_motor_b16_s *motor)
+static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
{
- struct foc_angle_in_b16_s ain;
- struct foc_angle_out_b16_s aout;
- int ret = OK;
+ int ret = OK;
DEBUGASSERT(motor);
- /* Update open-loop angle handler */
-
- ain.vel = motor->vel_set;
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
+ if (start == true)
+ {
+ /* Start motor if VBUS data present */
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_b16(&motor->openloop, &ain, &aout);
+ if (motor->mq.vbus > 0)
+ {
+ /* Configure motor controller */
- /* Store open-loop angle */
+ PRINTF("Configure motor %d!\n", motor->envp->id);
- motor->angle_ol = aout.angle;
+ ret = foc_motor_configure(motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
+ goto errout;
+ }
- /* Get phase angle now */
+ /* Start/stop FOC dev request */
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
+ motor->startstop = true;
+ }
}
else
-#endif
{
- /* TODO: get phase angle from observer or sensor */
+ /* Start/stop FOC dev request */
- ASSERT(0);
+ motor->startstop = true;
}
+errout:
return ret;
}
/****************************************************************************
- * Name: foc_motor_control
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_motor_init
****************************************************************************/
-static int foc_motor_control(FAR struct foc_motor_b16_s *motor)
+int foc_motor_init(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_ctrl_env_s *envp)
{
- int ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ struct foc_openloop_cfg_b16_s ol_cfg;
+#endif
+ int ret = OK;
DEBUGASSERT(motor);
+ DEBUGASSERT(envp);
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
+ /* Reset data */
- motor->vel_now = motor->vel_set;
+ memset(motor, 0, sizeof(struct foc_motor_b16_s));
- /* Run velocity ramp controller */
+ /* Connect envp with motor handler */
- ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
+ motor->envp = envp;
+
+ /* Initialize motor data */
+
+ motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+ motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Initialize open-loop angle handler */
+
+ foc_angle_init_b16(&motor->openloop,
+ &g_foc_angle_ol_b16);
+
+ /* Configure open-loop angle handler */
+
+ ol_cfg.per = motor->per;
+ foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
+#endif
-errout:
return ret;
}
/****************************************************************************
- * Name: foc_handler_run
+ * Name: foc_motor_deinit
****************************************************************************/
-static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_device_s *dev)
+int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
{
- struct foc_handler_input_b16_s input;
- struct foc_handler_output_b16_s output;
- b16_t current[CONFIG_MOTOR_FOC_PHASES];
- int ret = OK;
- int i = 0;
+ int ret = OK;
DEBUGASSERT(motor);
- DEBUGASSERT(dev);
-
- /* FOC device fault */
-
- if (motor->fault == true)
- {
- /* Stop motor */
-
- motor->dq_ref.q = 0;
- motor->dq_ref.d = 0;
- motor->angle_now = 0;
- motor->vbus = 0;
- /* Force velocity to zero */
-
- motor->vel_des = 0;
- }
-
- /* Get real currents */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Deinitialize PMSM model */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ ret = foc_model_deinit_b16(&motor->model);
+ if (ret < 0)
{
- current[i] = b16muli(motor->iphase_adc, dev->state.curr[i]);
+ PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
+ goto errout;
}
+#endif
- /* Get input for FOC handler */
-
- input.current = current;
- input.dq_ref = &motor->dq_ref;
- input.vdq_comp = &motor->vdq_comp;
- input.angle = motor->angle_now;
- input.vbus = motor->vbus;
- input.mode = motor->foc_mode;
-
- /* Run FOC controller */
-
- ret = foc_handler_run_b16(&motor->handler, &input, &output);
-
- /* Get duty from controller */
+ /* Deinitialize FOC handler */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ ret = foc_handler_deinit_b16(&motor->handler);
+ if (ret < 0)
{
- dev->params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
+ PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
+ goto errout;
}
- /* Get FOC handler state */
+ /* Reset data */
- foc_handler_state_b16(&motor->handler, &motor->foc_state);
+ memset(motor, 0, sizeof(struct foc_motor_b16_s));
+errout:
return ret;
}
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/****************************************************************************
- * Name: foc_model_state_get
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_model_state_get(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_device_s *dev)
+int foc_motor_get(FAR struct foc_motor_b16_s *motor)
{
- int i = 0;
+ struct foc_angle_in_b16_s ain;
+ struct foc_angle_out_b16_s aout;
+ int ret = OK;
DEBUGASSERT(motor);
- DEBUGASSERT(dev);
- /* Get model state */
+ /* Update open-loop angle handler */
+
+ ain.vel = motor->vel_set;
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ foc_angle_run_b16(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
- foc_model_state_b16(&motor->model, &motor->model_state);
+ motor->angle_ol = aout.angle;
- /* Get model currents */
+ /* Get phase angle now */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ if (motor->openloop_now == true)
{
- dev->state.curr[i] = motor->model_state.curr_raw[i];
+ motor->angle_now = motor->angle_ol;
}
+ else
+#endif
+ {
+ /* TODO: get phase angle from observer or sensor */
- return OK;
+ ASSERT(0);
+ }
+
+ return ret;
}
-#endif
-#ifdef FOC_STATE_PRINT_PRE
/****************************************************************************
- * Name: foc_state_print
+ * Name: foc_motor_control
****************************************************************************/
-static int foc_state_print(FAR struct foc_motor_b16_s *motor)
+int foc_motor_control(FAR struct foc_motor_b16_s *motor)
{
+ int ret = OK;
+
DEBUGASSERT(motor);
- PRINTF("b16 inst %d:\n", motor->envp->inst);
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
- foc_handler_state_print_b16(&motor->foc_state);
+ motor->vel_now = motor->vel_set;
- return OK;
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
}
-#endif
/****************************************************************************
* Name: foc_motor_handle
****************************************************************************/
-static int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
- FAR struct foc_mq_s *handle)
+int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_mq_s *handle)
{
int ret = OK;
@@ -743,228 +539,3 @@ static int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
errout:
return ret;
}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: foc_fixed16_thr
- ****************************************************************************/
-
-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;
-
- DEBUGASSERT(envp);
-
- PRINTFV("foc_fixed_thr, id=%d\n", envp->id);
-
- /* Reset data */
-
- memset(&handle, 0, sizeof(struct foc_mq_s));
-
- /* Initialize motor controller */
-
- ret = foc_motor_init(&motor, envp);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_init failed %d!\n", ret);
- goto errout;
- }
-
- /* Initialize FOC device as blocking */
-
- ret = foc_device_init(&dev, envp->id);
- if (ret < 0)
- {
- 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);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
- goto errout;
- }
-
- /* Start with motor free */
-
- handle.app_state = FOC_EXAMPLE_STATE_FREE;
-
- /* Wait some time */
-
- usleep(1000);
-
- /* Control loop */
-
- while (motor.mq.quit == false)
- {
- PRINTFV("foc_fixed16_thr %d %d\n", envp->id, time);
-
- /* Handle mqueue */
-
- ret = foc_mq_handle(envp->mqd, &handle);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_mq_handle failed %d!\n", ret);
- goto errout;
- }
-
- /* Handle motor data */
-
- ret = foc_motor_handle(&motor, &handle);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_handle failed %d!\n", ret);
- 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(&dev);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
- 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 */
-
- 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 */
-
- ret = foc_motor_get(&motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
- goto errout;
- }
-
- /* Motor control */
-
- ret = foc_motor_control(&motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
- goto errout;
- }
-
- /* Run FOC */
-
- 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 */
-
- if (time % FOC_STATE_PRINT_PRE == 0)
- {
- foc_state_print(&motor);
- }
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Feed FOC model with data */
-
- foc_model_run_b16(&motor.model,
- ftob16(FOC_MODEL_LOAD),
- &motor.foc_state.vab);
-#endif
-
- /* 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;
- }
- }
- else
- {
- usleep(1000);
- }
-
- /* Increase counter */
-
- time += 1;
- }
-
-errout:
-
- /* Deinit motor controller */
-
- ret = foc_motor_deinit(&motor);
- if (ret != OK)
- {
- PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret);
- }
-
- PRINTF("Stop FOC device %d!\n", envp->id);
-
- /* De-initialize FOC device */
-
- ret = foc_device_deinit(&dev);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret);
- }
-
- PRINTF("foc_fixed16_thr %d exit\n", envp->id);
-
- return ret;
-}
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
new file mode 100644
index 0000000..0be4aa8
--- /dev/null
+++ b/examples/foc/foc_motor_b16.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ * apps/examples/foc/foc_motor_b16.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __EXAMPLES_FOC_FOC_MOTOR_B16_H
+#define __EXAMPLES_FOC_FOC_MOTOR_B16_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+
+#include "industry/foc/fixed16/foc_handler.h"
+#include "industry/foc/fixed16/foc_ramp.h"
+#include "industry/foc/fixed16/foc_angle.h"
+#include "industry/foc/fixed16/foc_velocity.h"
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+# include "industry/foc/fixed16/foc_model.h"
+#endif
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* FOC motor data (fixed16) */
+
+struct foc_motor_b16_s
+{
+ FAR struct foc_ctrl_env_s *envp; /* Thread env */
+ 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 */
+ foc_angle_b16_t openloop; /* Open-loop angle handler */
+#endif
+ int foc_mode; /* FOC mode */
+ b16_t vbus; /* Power bus voltage */
+ b16_t angle_now; /* Phase angle now */
+ b16_t vel_set; /* Velocity setting now */
+ b16_t vel_now; /* Velocity now */
+ b16_t vel_des; /* Velocity destination */
+ 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 */
+ struct foc_mq_s mq; /* MQ data */
+ struct foc_state_b16_s foc_state; /* FOC controller sate */
+ struct foc_ramp_b16_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_b16_s model; /* Model handler */
+ struct foc_model_state_b16_s model_state; /* PMSM model state */
+#endif
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+int foc_motor_init(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_ctrl_env_s *envp);
+int foc_motor_deinit(FAR struct foc_motor_b16_s *motor);
+int foc_motor_get(FAR struct foc_motor_b16_s *motor);
+int foc_motor_control(FAR struct foc_motor_b16_s *motor);
+int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_mq_s *handle);
+
+#endif /* __EXAMPLES_FOC_FOC_MOTOR_B16_H */
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_motor_f32.c
similarity index 53%
copy from examples/foc/foc_float_thr.c
copy to examples/foc/foc_motor_f32.c
index fae5382..3c9536b 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_motor_f32.c
@@ -1,5 +1,5 @@
/****************************************************************************
- * apps/examples/foc/foc_float_thr.c
+ * apps/examples/foc/foc_motor_f32.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
@@ -24,167 +24,27 @@
#include <nuttx/config.h>
-#include <stdio.h>
-#include <fcntl.h>
#include <assert.h>
-#include <dsp.h>
+#include "foc_motor_f32.h"
-#include "foc_mq.h"
-#include "foc_thr.h"
#include "foc_cfg.h"
#include "foc_adc.h"
-
#include "foc_debug.h"
-#include "industry/foc/foc_utils.h"
-#include "industry/foc/foc_common.h"
-#include "industry/foc/float/foc_handler.h"
-#include "industry/foc/float/foc_ramp.h"
-#include "industry/foc/float/foc_angle.h"
-#include "industry/foc/float/foc_velocity.h"
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
-# include "industry/foc/float/foc_model.h"
-#endif
-
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
-#ifndef CONFIG_INDUSTRY_FOC_FLOAT
-# error
-#endif
-
/****************************************************************************
* Private Type Definition
****************************************************************************/
-/* FOC motor data */
-
-struct foc_motor_f32_s
-{
- FAR struct foc_ctrl_env_s *envp; /* Thread env */
- 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 */
- foc_angle_f32_t openloop; /* Open-loop angle handler */
-#endif
- int foc_mode; /* FOC mode */
- float vbus; /* Power bus voltage */
- float angle_now; /* Phase angle now */
- float vel_set; /* Velocity setting now */
- float vel_now; /* Velocity now */
- float vel_des; /* Velocity destination */
- 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 */
- struct foc_mq_s mq; /* MQ data */
- struct foc_state_f32_s foc_state; /* FOC controller sate */
- struct foc_ramp_f32_s ramp; /* Velocity ramp data */
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- struct foc_model_f32_s model; /* Model handler */
- struct foc_model_state_f32_s model_state; /* PMSM model state */
-#endif
-};
-
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
- * Name: foc_motor_init
- ****************************************************************************/
-
-static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_ctrl_env_s *envp)
-{
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- struct foc_openloop_cfg_f32_s ol_cfg;
-#endif
- int ret = OK;
-
- DEBUGASSERT(motor);
- DEBUGASSERT(envp);
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_f32_s));
-
- /* Connect envp with motor handler */
-
- motor->envp = envp;
-
- /* Initialize motor data */
-
- motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
- motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Initialize open-loop angle handler */
-
- foc_angle_init_f32(&motor->openloop,
- &g_foc_angle_ol_f32);
-
- /* Configure open-loop angle handler */
-
- ol_cfg.per = motor->per;
- foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
-#endif
-
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_mode_init
- ****************************************************************************/
-
-static int foc_mode_init(FAR struct foc_motor_f32_s *motor)
-{
- int ret = OK;
-
- switch (motor->envp->mode)
- {
- case FOC_OPMODE_IDLE:
- {
- motor->foc_mode = FOC_HANDLER_MODE_IDLE;
- break;
- }
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- case FOC_OPMODE_OL_V_VEL:
- {
- motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
- motor->openloop_now = true;
- break;
- }
-
- case FOC_OPMODE_OL_C_VEL:
- {
- motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
- motor->openloop_now = true;
- break;
- }
-#endif
-
- default:
- {
- PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
- ret = -EINVAL;
- goto errout;
- }
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_motor_configure
****************************************************************************/
@@ -279,94 +139,6 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_start
- ****************************************************************************/
-
-static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
- if (start == true)
- {
- /* Start motor if VBUS data present */
-
- if (motor->mq.vbus > 0)
- {
- /* Configure motor controller */
-
- PRINTF("Configure motor %d!\n", motor->envp->id);
-
- ret = foc_motor_configure(motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
- goto errout;
- }
-
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
- else
- {
- /* Return error if no VBUS data */
-
- PRINTF("ERROR: start request without VBUS!\n");
- goto errout;
- }
- }
- else
- {
- /* Start/stop FOC dev request */
-
- motor->startstop = true;
- }
-
-errout:
- return ret;
-}
-
-/****************************************************************************
- * Name: foc_motor_deinit
- ****************************************************************************/
-
-static int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
-{
- int ret = OK;
-
- DEBUGASSERT(motor);
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Deinitialize PMSM model */
-
- ret = foc_model_deinit_f32(&motor->model);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
- goto errout;
- }
-#endif
-
- /* Deinitialize FOC handler */
-
- ret = foc_handler_deinit_f32(&motor->handler);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
- goto errout;
- }
-
- /* Reset data */
-
- memset(motor, 0, sizeof(struct foc_motor_f32_s));
-
-errout:
- return ret;
-}
-
-/****************************************************************************
* Name: foc_motor_vbus
****************************************************************************/
@@ -469,195 +241,218 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_get
+ * Name: foc_motor_start
****************************************************************************/
-static int foc_motor_get(FAR struct foc_motor_f32_s *motor)
+static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
{
- struct foc_angle_in_f32_s ain;
- struct foc_angle_out_f32_s aout;
- int ret = OK;
+ int ret = OK;
DEBUGASSERT(motor);
- /* Update open-loop angle handler */
+ if (start == true)
+ {
+ /* Start motor if VBUS data present */
- ain.vel = motor->vel_set;
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
+ if (motor->mq.vbus > 0)
+ {
+ /* Configure motor controller */
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_f32(&motor->openloop, &ain, &aout);
+ PRINTF("Configure motor %d!\n", motor->envp->id);
- /* Store open-loop angle */
+ ret = foc_motor_configure(motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
+ goto errout;
+ }
- motor->angle_ol = aout.angle;
+ /* Start/stop FOC dev request */
- /* Get phase angle now */
+ motor->startstop = true;
+ }
+ else
+ {
+ /* Return error if no VBUS data */
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
+ PRINTF("ERROR: start request without VBUS!\n");
+ goto errout;
+ }
}
else
-#endif
{
- /* TODO: get phase angle from observer or sensor */
+ /* Start/stop FOC dev request */
- ASSERT(0);
+ motor->startstop = true;
}
+errout:
return ret;
}
/****************************************************************************
- * Name: foc_motor_control
+ * Public Functions
****************************************************************************/
-static int foc_motor_control(FAR struct foc_motor_f32_s *motor)
+/****************************************************************************
+ * Name: foc_motor_init
+ ****************************************************************************/
+
+int foc_motor_init(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_ctrl_env_s *envp)
{
- int ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ struct foc_openloop_cfg_f32_s ol_cfg;
+#endif
+ int ret = OK;
DEBUGASSERT(motor);
+ DEBUGASSERT(envp);
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
+ /* Reset data */
- motor->vel_now = motor->vel_set;
+ memset(motor, 0, sizeof(struct foc_motor_f32_s));
- /* Run velocity ramp controller */
+ /* Connect envp with motor handler */
- ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
+ motor->envp = envp;
+
+ /* Initialize motor data */
+
+ motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+ motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Initialize open-loop angle handler */
+
+ foc_angle_init_f32(&motor->openloop,
+ &g_foc_angle_ol_f32);
+
+ /* Configure open-loop angle handler */
+
+ ol_cfg.per = motor->per;
+ foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
+#endif
-errout:
return ret;
}
/****************************************************************************
- * Name: foc_handler_run
+ * Name: foc_motor_deinit
****************************************************************************/
-static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_device_s *dev)
+int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
{
- struct foc_handler_input_f32_s input;
- struct foc_handler_output_f32_s output;
- float current[CONFIG_MOTOR_FOC_PHASES];
- int ret = OK;
- int i = 0;
+ int ret = OK;
DEBUGASSERT(motor);
- DEBUGASSERT(dev);
-
- /* FOC device fault */
-
- if (motor->fault == true)
- {
- /* Stop motor */
-
- motor->dq_ref.q = 0;
- motor->dq_ref.d = 0;
- motor->angle_now = 0;
- motor->vbus = 0;
-
- /* Force velocity to zero */
-
- motor->vel_des = 0;
- }
- /* Get real currents */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Deinitialize PMSM model */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ ret = foc_model_deinit_f32(&motor->model);
+ if (ret < 0)
{
- current[i] = (motor->iphase_adc * dev->state.curr[i]);
+ PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
+ goto errout;
}
+#endif
- /* Get input for FOC handler */
-
- input.current = current;
- input.dq_ref = &motor->dq_ref;
- input.vdq_comp = &motor->vdq_comp;
- input.angle = motor->angle_now;
- input.vbus = motor->vbus;
- input.mode = motor->foc_mode;
-
- /* Run FOC controller */
-
- ret = foc_handler_run_f32(&motor->handler, &input, &output);
-
- /* Get duty from controller */
+ /* Deinitialize FOC handler */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ ret = foc_handler_deinit_f32(&motor->handler);
+ if (ret < 0)
{
- dev->params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
+ PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
+ goto errout;
}
- /* Get FOC handler state */
+ /* Reset data */
- foc_handler_state_f32(&motor->handler, &motor->foc_state);
+ memset(motor, 0, sizeof(struct foc_motor_f32_s));
+errout:
return ret;
}
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/****************************************************************************
- * Name: foc_model_state_get
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_model_state_get(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_device_s *dev)
+int foc_motor_get(FAR struct foc_motor_f32_s *motor)
{
- int i = 0;
+ struct foc_angle_in_f32_s ain;
+ struct foc_angle_out_f32_s aout;
+ int ret = OK;
DEBUGASSERT(motor);
- DEBUGASSERT(dev);
- /* Get model state */
+ /* Update open-loop angle handler */
+
+ ain.vel = motor->vel_set;
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ foc_angle_run_f32(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
- foc_model_state_f32(&motor->model, &motor->model_state);
+ motor->angle_ol = aout.angle;
- /* Get model currents */
+ /* Get phase angle now */
- for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ if (motor->openloop_now == true)
{
- dev->state.curr[i] = motor->model_state.curr_raw[i];
+ motor->angle_now = motor->angle_ol;
}
-
- return OK;
-}
+ else
#endif
+ {
+ /* TODO: get phase angle from observer or sensor */
-#ifdef FOC_STATE_PRINT_PRE
+ ASSERT(0);
+ }
+
+ return ret;
+}
/****************************************************************************
- * Name: foc_state_print
+ * Name: foc_motor_control
****************************************************************************/
-static int foc_state_print(FAR struct foc_motor_f32_s *motor)
+int foc_motor_control(FAR struct foc_motor_f32_s *motor)
{
+ int ret = OK;
+
DEBUGASSERT(motor);
- PRINTF("f32 inst %d:\n", motor->envp->inst);
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
- foc_handler_state_print_f32(&motor->foc_state);
+ motor->vel_now = motor->vel_set;
- return OK;
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
}
-#endif
/****************************************************************************
* Name: foc_motor_handle
****************************************************************************/
-static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
- FAR struct foc_mq_s *handle)
+int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_mq_s *handle)
{
int ret = OK;
@@ -746,228 +541,3 @@ static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
errout:
return ret;
}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: foc_float_thr
- ****************************************************************************/
-
-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;
-
- DEBUGASSERT(envp);
-
- PRINTFV("foc_float_thr, id=%d\n", envp->id);
-
- /* Reset data */
-
- memset(&handle, 0, sizeof(struct foc_mq_s));
-
- /* Initialize motor controller */
-
- ret = foc_motor_init(&motor, envp);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_init failed %d!\n", ret);
- goto errout;
- }
-
- /* Initialize FOC device as blocking */
-
- ret = foc_device_init(&dev, envp->id);
- if (ret < 0)
- {
- 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);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
- goto errout;
- }
-
- /* Start with motor free */
-
- handle.app_state = FOC_EXAMPLE_STATE_FREE;
-
- /* Wait some time */
-
- usleep(1000);
-
- /* Control loop */
-
- while (motor.mq.quit == false)
- {
- PRINTFV("foc_float_thr %d %d\n", envp->id, time);
-
- /* Handle mqueue */
-
- ret = foc_mq_handle(envp->mqd, &handle);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_mq_handle failed %d!\n", ret);
- goto errout;
- }
-
- /* Handle motor data */
-
- ret = foc_motor_handle(&motor, &handle);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_handle failed %d!\n", ret);
- 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(&dev);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
- 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 */
-
- 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 */
-
- ret = foc_motor_get(&motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
- goto errout;
- }
-
- /* Motor control */
-
- ret = foc_motor_control(&motor);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
- goto errout;
- }
-
- /* Run FOC */
-
- 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 */
-
- if (time % FOC_STATE_PRINT_PRE == 0)
- {
- foc_state_print(&motor);
- }
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
- /* Feed FOC model with data */
-
- foc_model_run_f32(&motor.model,
- FOC_MODEL_LOAD,
- &motor.foc_state.vab);
-#endif
-
- /* 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;
- }
- }
- else
- {
- usleep(1000);
- }
-
- /* Increase counter */
-
- time += 1;
- }
-
-errout:
-
- /* Deinit motor controller */
-
- ret = foc_motor_deinit(&motor);
- if (ret != OK)
- {
- PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret);
- }
-
- PRINTF("Stop FOC device %d!\n", envp->id);
-
- /* De-initialize FOC device */
-
- ret = foc_device_deinit(&dev);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret);
- }
-
- PRINTF("foc_float_thr %d exit\n", envp->id);
-
- return ret;
-}
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
new file mode 100644
index 0000000..1ab5f9c
--- /dev/null
+++ b/examples/foc/foc_motor_f32.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+ * apps/examples/foc/foc_motor_f32.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __EXAMPLES_FOC_FOC_MOTOR_F32_H
+#define __EXAMPLES_FOC_FOC_MOTOR_F32_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+
+#include "industry/foc/float/foc_handler.h"
+#include "industry/foc/float/foc_ramp.h"
+#include "industry/foc/float/foc_angle.h"
+#include "industry/foc/float/foc_velocity.h"
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+# include "industry/foc/float/foc_model.h"
+#endif
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* FOC motor data (float32) */
+
+struct foc_motor_f32_s
+{
+ FAR struct foc_ctrl_env_s *envp; /* Thread env */
+ 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 */
+ foc_angle_f32_t openloop; /* Open-loop angle handler */
+#endif
+ int foc_mode; /* FOC mode */
+ float vbus; /* Power bus voltage */
+ float angle_now; /* Phase angle now */
+ float vel_set; /* Velocity setting now */
+ float vel_now; /* Velocity now */
+ float vel_des; /* Velocity destination */
+ 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 */
+ struct foc_mq_s mq; /* MQ data */
+ struct foc_state_f32_s foc_state; /* FOC controller sate */
+ struct foc_ramp_f32_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_f32_s model; /* Model handler */
+ struct foc_model_state_f32_s model_state; /* PMSM model state */
+#endif
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+int foc_motor_init(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_ctrl_env_s *envp);
+int foc_motor_deinit(FAR struct foc_motor_f32_s *motor);
+int foc_motor_get(FAR struct foc_motor_f32_s *motor);
+int foc_motor_control(FAR struct foc_motor_f32_s *motor);
+int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_mq_s *handle);
+
+#endif /* __EXAMPLES_FOC_FOC_MOTOR_F32_H */
[incubator-nuttx-apps] 02/05: examples/foc: open FOC device in FOC
threads
Posted by xi...@apache.org.
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 33b34f88522547d1ab5d41805b25b167348ba90a
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 10:35:43 2021 +0100
examples/foc: open FOC device in FOC threads
---
examples/foc/foc_fixed16_thr.c | 52 ++++++++++++++++++++++++++++--------------
examples/foc/foc_float_thr.c | 50 +++++++++++++++++++++++++++-------------
examples/foc/foc_main.c | 17 --------------
examples/foc/foc_thr.h | 1 -
4 files changed, 69 insertions(+), 51 deletions(-)
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index b41f313..44fb1e6 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -64,6 +64,7 @@
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 */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */
@@ -121,15 +122,6 @@ static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
motor->envp = envp;
- /* Get device info */
-
- ret = foc_dev_getinfo(envp->dev.fd, &motor->info);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
- goto errout;
- }
-
/* Initialize motor data */
motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
@@ -297,7 +289,7 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
/* Configure FOC device */
- ret = foc_dev_setcfg(motor->envp->dev.fd, &cfg);
+ ret = foc_dev_setcfg(motor->dev.fd, &cfg);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_setcfg %d!\n", ret);
@@ -339,7 +331,7 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
PRINTF("Start FOC device %d!\n", motor->envp->id);
- ret = foc_dev_start(motor->envp->dev.fd);
+ ret = foc_dev_start(motor->dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_start failed %d!\n", ret);
@@ -353,7 +345,7 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
PRINTF("Stop FOC device %d!\n", motor->envp->id);
- ret = foc_dev_stop(motor->envp->dev.fd);
+ ret = foc_dev_stop(motor->dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_stop failed %d!\n", ret);
@@ -663,7 +655,7 @@ static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
/* Get FOC state - blocking */
- ret = foc_dev_getstate(motor->envp->dev.fd, &motor->dev_state);
+ ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret);
@@ -699,7 +691,7 @@ static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor)
/* Write FOC parameters */
- ret = foc_dev_setparams(motor->envp->dev.fd, &motor->dev_params);
+ ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret);
@@ -871,6 +863,24 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout;
}
+ /* Open FOC device as blocking */
+
+ ret = foc_device_open(&motor.dev, envp->id);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_open failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get device info */
+
+ ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
+ goto errout;
+ }
+
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -930,7 +940,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
ret = foc_state_handle(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_state_handle failed %d!\n", ret);
+ PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
@@ -938,7 +948,7 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
{
/* Clear fault state */
- ret = foc_dev_clearfault(envp->dev.fd);
+ ret = foc_dev_clearfault(motor.dev.fd);
if (ret != OK)
{
PRINTF("ERROR: foc_dev_clearfault failed %d!\n", ret);
@@ -1023,12 +1033,20 @@ errout:
/* Stop FOC device */
- ret = foc_dev_stop(envp->dev.fd);
+ ret = foc_dev_stop(motor.dev.fd);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_stop %d!\n", ret);
}
+ /* Close FOC control device */
+
+ ret = foc_device_close(&motor.dev);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret);
+ }
+
PRINTF("foc_fixed16_thr %d exit\n", envp->id);
return ret;
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 3a5ef25..1386ae8 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -64,6 +64,7 @@
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 */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
bool openloop_now; /* Open-loop now */
@@ -121,15 +122,6 @@ static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
motor->envp = envp;
- /* Get device info */
-
- ret = foc_dev_getinfo(envp->dev.fd, &motor->info);
- if (ret < 0)
- {
- PRINTFV("ERROR: foc_dev_getinfo failed %d!\n", ret);
- goto errout;
- }
-
/* Initialize motor data */
motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
@@ -297,7 +289,7 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
/* Configure FOC device */
- ret = foc_dev_setcfg(motor->envp->dev.fd, &cfg);
+ ret = foc_dev_setcfg(motor->dev.fd, &cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setcfg %d!\n", ret);
@@ -339,7 +331,7 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
PRINTF("Start FOC device %d!\n", motor->envp->id);
- ret = foc_dev_start(motor->envp->dev.fd);
+ ret = foc_dev_start(motor->dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
@@ -360,7 +352,7 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
PRINTF("Stop FOC device %d!\n", motor->envp->id);
- ret = foc_dev_stop(motor->envp->dev.fd);
+ ret = foc_dev_stop(motor->dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
@@ -663,7 +655,7 @@ static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
/* Get FOC state - blocking */
- ret = foc_dev_getstate(motor->envp->dev.fd, &motor->dev_state);
+ ret = foc_dev_getstate(motor->dev.fd, &motor->dev_state);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
@@ -699,7 +691,7 @@ static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor)
/* Write FOC parameters */
- ret = foc_dev_setparams(motor->envp->dev.fd, &motor->dev_params);
+ ret = foc_dev_setparams(motor->dev.fd, &motor->dev_params);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
@@ -874,6 +866,24 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
goto errout;
}
+ /* Open FOC device as blocking */
+
+ ret = foc_device_open(&motor.dev, envp->id);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_open failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get device info */
+
+ ret = foc_dev_getinfo(motor.dev.fd, &motor.info);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
+ goto errout;
+ }
+
/* Initialize controller mode */
ret = foc_mode_init(&motor);
@@ -941,7 +951,7 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
{
/* Clear fault state */
- ret = foc_dev_clearfault(envp->dev.fd);
+ ret = foc_dev_clearfault(motor.dev.fd);
if (ret != OK)
{
goto errout;
@@ -1025,12 +1035,20 @@ errout:
/* Stop FOC device */
- ret = foc_dev_stop(envp->dev.fd);
+ ret = foc_dev_stop(motor.dev.fd);
if (ret < 0)
{
PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
}
+ /* Close FOC control device */
+
+ ret = foc_device_close(&motor.dev);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret);
+ }
+
PRINTF("foc_float_thr %d exit\n", envp->id);
return ret;
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index cfe72e1..61f7062 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -303,15 +303,6 @@ FAR void *foc_control_thr(FAR void *arg)
DEBUGASSERT(envp);
- /* Open FOC device as blocking */
-
- ret = foc_device_open(&envp->dev, envp->id);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_device_open failed %d!\n", ret);
- goto errout;
- }
-
/* Get controller type */
pthread_mutex_lock(&g_cntr_lock);
@@ -411,14 +402,6 @@ FAR void *foc_control_thr(FAR void *arg)
errout:
- /* Close FOC control device */
-
- ret = foc_device_close(&envp->dev);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret);
- }
-
/* Close queue */
if (envp->mqd == (mqd_t)-1)
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index 0797fbb..a714a3d 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -70,7 +70,6 @@ enum foc_operation_mode_e
struct foc_ctrl_env_s
{
- struct foc_device_s dev; /* FOC device */
mqd_t mqd; /* Control msg queue */
int id; /* FOC device id */
int inst; /* Type specific instance counter */
[incubator-nuttx-apps] 01/05: examples/foc: separate motor control
logic from motor state logic
Posted by xi...@apache.org.
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 2d034ed09af39b98b01a74ac9f209314a124f76f
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Oct 30 14:45:03 2021 +0200
examples/foc: separate motor control logic from motor state logic
---
examples/foc/foc_fixed16_thr.c | 64 ++++++++++++++++++++++++++++--------------
examples/foc/foc_float_thr.c | 64 ++++++++++++++++++++++++++++--------------
2 files changed, 86 insertions(+), 42 deletions(-)
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 6b6fc98..b41f313 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_run
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
+static int foc_motor_get(FAR struct foc_motor_b16_s *motor)
{
struct foc_angle_in_b16_s ain;
struct foc_angle_out_b16_s aout;
@@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
-
- motor->vel_now = motor->vel_set;
-
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
-
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
ASSERT(0);
}
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_control
+ ****************************************************************************/
+
+static int foc_motor_control(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
+
+ motor->vel_now = motor->vel_set;
+
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
errout:
return ret;
}
@@ -933,12 +946,21 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
}
}
- /* Run motor controller */
+ /* Get motor state */
+
+ ret = foc_motor_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Motor control */
- ret = foc_motor_run(&motor);
+ ret = foc_motor_control(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index e1a7eed..3a5ef25 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -513,10 +513,10 @@ errout:
}
/****************************************************************************
- * Name: foc_motor_run
+ * Name: foc_motor_get
****************************************************************************/
-static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
+static int foc_motor_get(FAR struct foc_motor_f32_s *motor)
{
struct foc_angle_in_f32_s ain;
struct foc_angle_out_f32_s aout;
@@ -524,22 +524,6 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
- /* No velocity feedback - assume that velocity now is velocity set
- * TODO: velocity observer or sensor
- */
-
- motor->vel_now = motor->vel_set;
-
- /* Run velocity ramp controller */
-
- ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
- motor->vel_now, &motor->vel_set);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
- goto errout;
- }
-
/* Update open-loop angle handler */
ain.vel = motor->vel_set;
@@ -567,6 +551,35 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
ASSERT(0);
}
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_control
+ ****************************************************************************/
+
+static int foc_motor_control(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
+
+ motor->vel_now = motor->vel_set;
+
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
errout:
return ret;
}
@@ -935,12 +948,21 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
}
}
- /* Run motor controller */
+ /* Get motor state */
+
+ ret = foc_motor_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Motor control */
- ret = foc_motor_run(&motor);
+ ret = foc_motor_control(&motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}