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

examples/foc: add Hall sensor support

Browse Source
This commit is contained in:
raiden00pl 2021-10-31 22:25:11 +01:00 committed by Xiang Xiao
parent c771942d8e
commit 5ee9572531
6 changed files with 139 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/foc/Kconfig
View File

@ -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

8
examples/foc/foc_cfg.h
View File

@ -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 */

59
examples/foc/foc_motor_b16.c
View File

@ -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;
}

4
examples/foc/foc_motor_b16.h
View File

@ -70,6 +70,10 @@ struct foc_motor_b16_s
bool openloop_now; /* Open-loop now */
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 */

59
examples/foc/foc_motor_f32.c
View File

@ -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;
}

4
examples/foc/foc_motor_f32.h
View File

@ -70,6 +70,10 @@ struct foc_motor_f32_s
bool openloop_now; /* Open-loop now */
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 */