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

examples/foc: add Qenco support

Browse Source
This commit is contained in:
raiden00pl 2021-11-06 18:44:30 +01:00 committed by Xiang Xiao
parent 171a3c41df
commit 15b66aa128
6 changed files with 167 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
examples/foc/Kconfig
View File

@ -90,10 +90,37 @@ endchoice #
if EXAMPLES_FOC_SENSORED
choice
prompt "FOC sensored sensor selection"
config EXAMPLES_FOC_HAVE_QENCO
bool "FOC example have qencoder"
select INDUSTRY_FOC_ANGLE_QENCO
config EXAMPLES_FOC_HAVE_HALL
bool "FOC example Hall sensor support"
select INDUSTRY_FOC_ANGLE_HALL
default n
endchoice # FOC sensored sensor selection
if EXAMPLES_FOC_HAVE_QENCO
config EXAMPLES_FOC_MOTOR_POLES
int "FOC example motor poles pairs"
default 0
config EXAMPLES_FOC_QENCO_POSMAX
int "FOC example qencoder maximum position"
default 0
config EXAMPLES_FOC_QENCO_DEVPATH
string "FOC example qencoder path prefix"
default "/dev/qe"
---help---
The default path to the qenco device without the device minor number.
Default: /dev/qenco
endif # EXAMPLES_FOC_HAVE_QENCO
if EXAMPLES_FOC_HAVE_HALL

11
examples/foc/foc_cfg.h
View File

@ -117,4 +117,15 @@
# endif
#endif
/* Qenco configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
# if CONFIG_EXAMPLES_FOC_MOTOR_POLES == 0
# error
# endif
# if CONFIG_EXAMPLES_FOC_MOTOR_POSMAX == 0
# error
# endif
#endif
#endif /* __EXAMPLES_FOC_FOC_CFG_H */

61
examples/foc/foc_motor_b16.c
View File

@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg);
#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
/* Configure motor poles */
motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Initialize PMSM model */
@ -668,6 +674,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_QENCO
struct foc_qenco_cfg_b16_s qe_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_b16_s hl_cfg;
#endif
@ -704,6 +713,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_QENCO
/* Initialize qenco angle handler */
ret = foc_angle_init_b16(&motor->qenco,
&g_foc_angle_qe_b16);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
goto errout;
}
/* Get qenco devpath */
sprintf(motor->qedpath,
"%s%d",
CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
motor->envp->id);
/* Configure qenco angle handler */
qe_cfg.posmax = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
qe_cfg.devpath = motor->qedpath;
ret = foc_angle_cfg_b16(&motor->qenco, &qe_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Initialize hall angle handler */
@ -758,6 +798,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
/* Connect align callbacks private data */
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
align_cfg.cb.priv = &motor->hall;
# endif
@ -846,6 +889,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
motor->angle_ol = aout.angle;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
ret = foc_angle_run_b16(&motor->qenco, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
if (ret < 0)
@ -867,9 +919,14 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
else if (aout.type == FOC_ANGLE_TYPE_MECH)
{
/* TODO */
/* Store mechanical angle */
ASSERT(0);
motor->angle_m = aout.angle;
/* Convert mechanical angle to electrical angle */
motor->angle_el = (b16muli(motor->angle_m,
motor->poles) % MOTOR_ANGLE_E_MAX_B16);
}
else

5
examples/foc/foc_motor_b16.h
View File

@ -74,6 +74,10 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
char hldpath[32]; /* Hall devpath */
foc_angle_b16_t hall; /* Hall angle handler */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
char qedpath[32]; /* Qenco devpath */
foc_angle_b16_t qenco; /* Qenco angle handler */
#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
@ -94,6 +98,7 @@ struct foc_motor_b16_s
b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */
b16_t pwm_duty_max; /* PWM duty max */
uint8_t poles; /* Motor poles */
dq_frame_b16_t dq_ref; /* DQ reference */
dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
foc_handler_b16_t handler; /* FOC controller */

61
examples/foc/foc_motor_f32.c
View File

@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
/* Configure motor poles */
motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Initialize PMSM model */
@ -654,6 +660,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_QENCO
struct foc_qenco_cfg_f32_s qe_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_f32_s hl_cfg;
#endif
@ -690,6 +699,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_QENCO
/* Initialize qenco angle handler */
ret = foc_angle_init_f32(&motor->qenco,
&g_foc_angle_qe_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Get qenco devpath */
sprintf(motor->qedpath,
"%s%d",
CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
motor->envp->id);
/* Configure qenco angle handler */
qe_cfg.posmax = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
qe_cfg.devpath = motor->qedpath;
ret = foc_angle_cfg_f32(&motor->qenco, &qe_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Initialize hall angle handler */
@ -744,6 +784,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
/* Connect align callbacks private data */
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
align_cfg.cb.priv = &motor->hall;
# endif
@ -843,6 +886,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
motor->angle_ol = aout.angle;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
ret = foc_angle_run_f32(&motor->qenco, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
if (ret < 0)
@ -864,9 +916,14 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
else if (aout.type == FOC_ANGLE_TYPE_MECH)
{
/* TODO */
/* Store mechanical angle */
ASSERT(0);
motor->angle_m = aout.angle;
/* Convert mechanical angle to electrical angle */
motor->angle_el = fmodf(motor->angle_m * motor->poles,
MOTOR_ANGLE_E_MAX);
}
else

5
examples/foc/foc_motor_f32.h
View File

@ -74,6 +74,10 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
char hldpath[32]; /* Hall devpath */
foc_angle_f32_t hall; /* Hall angle handler */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
char qedpath[32]; /* Qenco devpath */
foc_angle_f32_t qenco; /* Qenco angle handler */
#endif
int foc_mode; /* FOC mode */
int ctrl_state; /* Controller state */
@ -94,6 +98,7 @@ struct foc_motor_f32_s
float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */
float pwm_duty_max; /* PWM duty max */
uint8_t poles; /* Motor poles */
dq_frame_f32_t dq_ref; /* DQ reference */
dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
foc_handler_f32_t handler; /* FOC controller */