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/11/04 18:51:10 UTC
[incubator-nuttx-apps] 06/06: examples/foc: add Hall sensor support
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 5ee957253145fe6ce6123df5e65b0137387dc80d
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Oct 31 22:25:11 2021 +0100
examples/foc: add Hall sensor support
---
examples/foc/Kconfig | 20 +++++++++++++++
examples/foc/foc_cfg.h | 8 +++---
examples/foc/foc_motor_b16.c | 59 +++++++++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_b16.h | 4 +++
examples/foc/foc_motor_f32.c | 59 +++++++++++++++++++++++++++++++++++++++-----
examples/foc/foc_motor_f32.h | 4 +++
6 files changed, 139 insertions(+), 15 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 26995f1..bd0062a 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -88,6 +88,26 @@ config EXAMPLES_FOC_SENSORED
endchoice #
+if EXAMPLES_FOC_SENSORED
+
+config EXAMPLES_FOC_HAVE_HALL
+ bool "FOC example Hall sensor support"
+ select INDUSTRY_FOC_ANGLE_HALL
+ default n
+
+if EXAMPLES_FOC_HAVE_HALL
+
+config EXAMPLES_FOC_HALL_DEVPATH
+ string "FOC example Hall sensor path prefix"
+ default "/dev/hall"
+ ---help---
+ The default path to the Hall device without the device minor number.
+ Default: /dev/hall
+
+endif # EXAMPLES_FOC_HAVE_HALL
+
+endif # EXAMPLES_FOC_SENSORED
+
config EXAMPLES_FOC_HAVE_OPENLOOP
bool "FOC example have open-loop controller"
select INDUSTRY_FOC_ANGLE_OPENLOOP
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 138eef8..8803499 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -31,10 +31,12 @@
* Pre-processor Definitions
****************************************************************************/
-/* For now only open-loop supported */
+/* For now only torque mode supported for sensored */
-#ifndef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
-# error For now only open-loop supported
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
+# ifndef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+# error
+# endif
#endif
/* For now only sensorless velocity control supported */
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 1f67d20..e43bca2 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -566,21 +566,19 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
-# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Open-loop works only in velocity control mode */
if (motor->openloop_now == true)
{
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->mmode != FOC_MMODE_VEL)
+#endif
{
PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
ret = -EINVAL;
goto errout;
}
}
-# else
-# error
-# endif
#endif
/* Get previous DQ */
@@ -670,6 +668,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
struct foc_openloop_cfg_b16_s ol_cfg;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ struct foc_hall_cfg_b16_s hl_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_b16_s align_cfg;
#endif
@@ -703,6 +704,37 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ /* Initialize hall angle handler */
+
+ ret = foc_angle_init_b16(&motor->hall,
+ &g_foc_angle_hl_b16);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get hall devpath */
+
+ sprintf(motor->hldpath,
+ "%s%d",
+ CONFIG_EXAMPLES_FOC_HALL_DEVPATH,
+ motor->envp->id);
+
+ /* Configure hall angle handler */
+
+ hl_cfg.devpath = motor->hldpath;
+ hl_cfg.per = motor->per;
+
+ ret = foc_angle_cfg_b16(&motor->hall, &hl_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
@@ -724,9 +756,11 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
align_cfg.cb.zero = foc_align_zero_cb;
align_cfg.cb.dir = foc_align_dir_cb;
- /* TODO: Connect align callbacks private data */
+ /* Connect align callbacks private data */
- align_cfg.cb.priv = NULL;
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ align_cfg.cb.priv = &motor->hall;
+# endif
ret = foc_routine_cfg_b16(&motor->align, &align_cfg);
if (ret < 0)
@@ -740,7 +774,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
motor->ctrl_state = FOC_CTRL_STATE_INIT;
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
errout:
+#endif
return ret;
}
@@ -810,6 +846,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
motor->angle_ol = aout.angle;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_SENSORED
/* Handle angle from sensor */
@@ -861,7 +906,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
/* TODO: velocity observer or sensor */
}
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
errout:
+#endif
return ret;
}
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index aae0920..84d708a 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -71,6 +71,10 @@ struct foc_motor_b16_s
b16_t angle_ol; /* Phase angle open-loop */
foc_angle_b16_t openloop; /* Open-loop angle handler */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ char hldpath[32]; /* Hall devpath */
+ foc_angle_b16_t hall; /* Hall angle handler */
+#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
b16_t vbus; /* Power bus voltage */
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 5495338..c8eb3f3 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -552,21 +552,19 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
-# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Open-loop works only in velocity control mode */
if (motor->openloop_now == true)
{
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->mmode != FOC_MMODE_VEL)
+# endif
{
PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
ret = -EINVAL;
goto errout;
}
}
-# else
-# error
-# endif
#endif
/* Get previous DQ */
@@ -656,6 +654,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
struct foc_openloop_cfg_f32_s ol_cfg;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ struct foc_hall_cfg_f32_s hl_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_f32_s align_cfg;
#endif
@@ -689,6 +690,37 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ /* Initialize hall angle handler */
+
+ ret = foc_angle_init_f32(&motor->hall,
+ &g_foc_angle_hl_f32);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get hall devpath */
+
+ sprintf(motor->hldpath,
+ "%s%d",
+ CONFIG_EXAMPLES_FOC_HALL_DEVPATH,
+ motor->envp->id);
+
+ /* Configure hall angle handler */
+
+ hl_cfg.devpath = motor->hldpath;
+ hl_cfg.per = motor->per;
+
+ ret = foc_angle_cfg_f32(&motor->hall, &hl_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
@@ -710,9 +742,11 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
align_cfg.cb.zero = foc_align_zero_cb;
align_cfg.cb.dir = foc_align_dir_cb;
- /* TODO: Connect align callbacks private data */
+ /* Connect align callbacks private data */
- align_cfg.cb.priv = NULL;
+# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ align_cfg.cb.priv = &motor->hall;
+# endif
ret = foc_routine_cfg_f32(&motor->align, &align_cfg);
if (ret < 0)
@@ -726,7 +760,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
motor->ctrl_state = FOC_CTRL_STATE_INIT;
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
errout:
+#endif
return ret;
}
@@ -807,6 +843,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
motor->angle_ol = aout.angle;
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_SENSORED
/* Handle angle from sensor */
@@ -858,7 +903,9 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
/* TODO: velocity observer or sensor */
}
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
errout:
+#endif
return ret;
}
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index 80f2fa9..f69e41a 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -71,6 +71,10 @@ struct foc_motor_f32_s
float angle_ol; /* Phase angle open-loop */
foc_angle_f32_t openloop; /* Open-loop angle handler */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+ char hldpath[32]; /* Hall devpath */
+ foc_angle_f32_t hall; /* Hall angle handler */
+#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
float vbus; /* Power bus voltage */