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

examples/foc: add velocity observers

Browse Source
This commit is contained in:
raiden00pl 2022-11-05 11:00:19 +01:00 committed by Xiang Xiao
parent 9bcca69967
commit 8e0819df04
12 changed files with 537 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
examples/foc/Kconfig
View File

@ -189,6 +189,60 @@ config EXAMPLES_FOC_HAVE_POS
bool "FOC example position controller support"
default n
config EXAMPLES_FOC_VELOBS
bool "FOC example velocity observer support"
default n
if EXAMPLES_FOC_HAVE_VEL
config EXAMPLES_FOC_VELNOW_FILTER
int "FOC example velocity controller (x1000)"
default 990
endif # EXAMPLES_FOC_HAVE_VEL
if EXAMPLES_FOC_VELOBS
choice
prompt "FOC example velocity observer selection"
default EXAMPLES_FOC_VELOBS_DIV
config EXAMPLES_FOC_VELOBS_DIV
bool "FOC velocity DIV observer"
select INDUSTRY_FOC_VELOCITY_ODIV
config EXAMPLES_FOC_VELOBS_PLL
bool "FOC velocity PLL observer"
select INDUSTRY_FOC_VELOCITY_OPLL
endchoice # FOC example velocity observer selection
if EXAMPLES_FOC_VELOBS_DIV
config EXAMPLES_FOC_VELOBS_DIV_SAMPLES
int "FOC velocity DIV observer samples"
default 10
config EXAMPLES_FOC_VELOBS_DIV_FILTER
int "FOC velocity DIV observer filter (x1000)"
default 990
endif # INDUSTRY_FOC_VELOCITY_ODIV
if EXAMPLES_FOC_VELOBS_PLL
config EXAMPLES_FOC_VELOBS_PLL_KP
int "FOC velocity PLL observer Kp (x1)"
default 0
config EXAMPLES_FOC_VELOBS_PLL_KI
int "FOC velocity PLL observer Ki (x1)"
default 0
endif # EXAMPLES_FOC_VELOBS_PLL
endif # EXAMPLES_FOC_VELOBS
menu "FOC user input"
config EXAMPLES_FOC_HAVE_ADC

13
examples/foc/foc_cfg.h
View File

@ -244,6 +244,19 @@ struct foc_thr_cfg_s
uint32_t ident_ind_volt; /* Ident res voltage (x1000) */
uint32_t ident_ind_sec; /* Ident ind sec */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
uint32_t vel_filter; /* Velocity filter (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
uint32_t vel_pll_kp; /* Vel PLL observer Kp (x1000) */
uint32_t vel_pll_ki; /* Vel PLL observer Ki (x1000) */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
uint32_t vel_div_samples; /* Vel DIV observer samples */
uint32_t vel_div_filter; /* Vel DIV observer filter (x1000) */
#endif
};
#endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */

8
examples/foc/foc_fixed16_thr.c
View File

@ -228,12 +228,12 @@ static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs,
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
# warning not supported yet
i++;
ptr = (FAR b16_t *)&motor->vel_el;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
# warning not supported yet
i++;
ptr = (FAR b16_t *)&motor->vel_mech;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
ptr = (FAR b16_t *)&motor->vbus;

12
examples/foc/foc_float_thr.c
View File

@ -229,12 +229,12 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
# warning not supported yet
i++;
ptr = (FAR float *)&motor->vel_el;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
# warning not supported yet
i++;
ptr = (FAR float *)&motor->vel_mech;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
ptr = (FAR float *)&motor->vbus;
@ -278,6 +278,10 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
ptr = svm3_tmp;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
ptr = (FAR float *)&motor->vel_obs;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
nxscope_unlock(&nxs->nxs);
}

11
examples/foc/foc_main.c
View File

@ -108,6 +108,17 @@ struct args_s g_args =
.ident_res_sec = CONFIG_EXAMPLES_FOC_IDENT_RES_SEC,
.ident_ind_volt = CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE,
.ident_ind_sec = CONFIG_EXAMPLES_FOC_IDENT_IND_SEC,
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
.vel_filter = CONFIG_EXAMPLES_FOC_VELNOW_FILTER,
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
.vel_pll_kp = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP,
.vel_pll_ki = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI,
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
.vel_div_samples = CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES,
.vel_div_filter = CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER,
#endif
}
};

185
examples/foc/foc_motor_b16.c
View File

@ -704,9 +704,32 @@ errout:
static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor)
{
/* Empty for now */
int ret = OK;
return OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_b16(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_b16(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
/****************************************************************************
@ -831,6 +854,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_b16_s hl_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_b16_cfg_s odiv_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
struct foc_vel_pll_b16_cfg_s opll_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_b16_s align_cfg;
#endif
@ -944,6 +973,56 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */
ret = foc_velocity_init_b16(&motor->vel_div,
&g_foc_velocity_odiv_b16);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
odiv_cfg.samples = (motor->envp->cfg->vel_div_samples);
odiv_cfg.filter = ftob16(motor->envp->cfg->vel_div_samples / 1000.0f);
odiv_cfg.per = motor->per;
ret = foc_velocity_cfg_b16(&motor->vel_div, &odiv_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Initialize velocity observer */
ret = foc_velocity_init_b16(&motor->vel_pll,
&g_foc_velocity_opll_b16);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
opll_cfg.kp = ftob16(motor->envp->cfg->vel_pll_kp / 1.0f);
opll_cfg.ki = ftob16(motor->envp->cfg->vel_pll_ki / 1.0f);
opll_cfg.per = motor->per;
ret = foc_velocity_cfg_b16(&motor->vel_pll, &opll_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
@ -1016,6 +1095,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Store velocity filter value */
motor->vel_filter = ftob16(motor->envp->cfg->vel_filter / 1000.0f);
#endif
/* Initialize controller state */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
@ -1087,6 +1172,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */
ret = foc_velocity_deinit_b16(&motor->vel_div);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Deinitialize PLL observer handler */
ret = foc_velocity_deinit_b16(&motor->vel_pll);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Deinitialize motor alignment routine */
@ -1143,9 +1250,13 @@ errout:
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;
struct foc_angle_in_b16_s ain;
struct foc_angle_out_b16_s aout;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_velocity_in_b16_s vin;
struct foc_velocity_out_b16_s vout;
#endif
int ret = OK;
DEBUGASSERT(motor);
@ -1202,7 +1313,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
/* Convert mechanical angle to electrical angle */
motor->angle_el = (b16muli(motor->angle_m,
motor->phy.poles) %
motor->phy.p) %
MOTOR_ANGLE_E_MAX_B16);
}
@ -1227,20 +1338,74 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
motor->angle_now = motor->angle_el;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Update velocity handler input data */
vin.state = &motor->foc_state;
vin.angle = motor->angle_now;
vin.vel = motor->vel.now;
vin.dir = motor->dir;
/* Get velocity */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_run_b16(&motor->vel_div, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_run_b16(&motor->vel_pll, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
#endif
/* Get motor electrical velocity now */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
if (motor->openloop_now == true)
{
/* No velocity feedback - assume that velocity now is velocity set */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
/* Electrical velocity from observer */
motor->vel.now = motor->vel.set;
motor->vel_el = motor->vel_obs;
#else
UNUSED(vin);
UNUSED(vout);
/* No velocity feedback - assume that electical velocity is
* velocity set
*/
motor->vel_el = motor->vel.set;
#endif
}
else
#endif
{
/* TODO: velocity observer or sensor */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
/* Store electrical velocity */
motor->vel_el = motor->vel_obs;
#else
ASSERT(0);
#endif
}
#ifdef CONFIG_EXAMPLES_FOC_SENSORED
LP_FILTER_B16(motor->vel.now, motor->vel_el, motor->vel_filter);
/* Get mechanical velocity */
motor->vel_mech = b16mulb16(motor->vel_el, motor->phy.one_by_p);
#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS)
errout:
#endif
return ret;

17
examples/foc/foc_motor_b16.h
View File

@ -102,6 +102,17 @@ struct foc_motor_b16_s
b16_t angle_m; /* Motor mechanical angle */
b16_t angle_el; /* Motor electrical angle */
/* Velocity state *********************************************************/
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
b16_t vel_el; /* Velocity - electrical */
b16_t vel_mech; /* Velocity - mechanical */
b16_t vel_filter; /* Velocity low-pass filter */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
b16_t vel_obs; /* Velocity observer output */
#endif
/* Motor setpoints ********************************************************/
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@ -150,6 +161,12 @@ struct foc_motor_b16_s
foc_angle_b16_t qenco; /* Qenco angle handler */
char qedpath[32]; /* Qenco devpath */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
foc_velocity_b16_t vel_div; /* DIV velocity observer */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_b16_t vel_pll; /* PLL velocity observer */
#endif
};
/****************************************************************************

191
examples/foc/foc_motor_f32.c
View File

@ -688,9 +688,32 @@ errout:
static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
{
/* Empty for now */
int ret = OK;
return OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_f32(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
/****************************************************************************
@ -815,6 +838,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_f32_s hl_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_f32_cfg_s odiv_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
struct foc_vel_pll_f32_cfg_s opll_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_f32_s align_cfg;
#endif
@ -928,6 +957,56 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */
ret = foc_velocity_init_f32(&motor->vel_div,
&g_foc_velocity_odiv_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
odiv_cfg.samples = (motor->envp->cfg->vel_div_samples);
odiv_cfg.filter = (motor->envp->cfg->vel_div_samples / 1000.0f);
odiv_cfg.per = motor->per;
ret = foc_velocity_cfg_f32(&motor->vel_div, &odiv_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Initialize velocity observer */
ret = foc_velocity_init_f32(&motor->vel_pll,
&g_foc_velocity_opll_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
opll_cfg.kp = (motor->envp->cfg->vel_pll_kp / 1.0f);
opll_cfg.ki = (motor->envp->cfg->vel_pll_ki / 1.0f);
opll_cfg.per = motor->per;
ret = foc_velocity_cfg_f32(&motor->vel_pll, &opll_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
@ -997,6 +1076,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Store velocity filter value */
motor->vel_filter = motor->envp->cfg->vel_filter / 1000.0f;
#endif
/* Initialize controller state */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
@ -1073,6 +1158,28 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */
ret = foc_velocity_deinit_f32(&motor->vel_div);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Deinitialize PLL observer handler */
ret = foc_velocity_deinit_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Deinitialize motor alignment routine */
@ -1129,9 +1236,13 @@ errout:
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;
struct foc_angle_in_f32_s ain;
struct foc_angle_out_f32_s aout;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
struct foc_velocity_in_f32_s vin;
struct foc_velocity_out_f32_s vout;
#endif
int ret = OK;
DEBUGASSERT(motor);
@ -1187,7 +1298,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
/* Convert mechanical angle to electrical angle */
motor->angle_el = fmodf(motor->angle_m * motor->phy.poles,
motor->angle_el = fmodf(motor->angle_m * motor->phy.p,
MOTOR_ANGLE_E_MAX);
}
@ -1212,20 +1323,80 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
motor->angle_now = motor->angle_el;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Update velocity handler input data */
vin.state = &motor->foc_state;
vin.angle = motor->angle_now;
vin.vel = motor->vel.now;
vin.dir = motor->dir;
/* Get velocity */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_run_f32(&motor->vel_div, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
motor->vel_obs = vout.velocity;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_run_f32(&motor->vel_pll, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
motor->vel_obs = vout.velocity;
#endif
/* Get motor electrical velocity now */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
if (motor->openloop_now == true)
{
/* No velocity feedback - assume that velocity now is velocity set */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
/* Electrical velocity from observer */
motor->vel.now = motor->vel.set;
motor->vel_el = motor->vel_obs;
#else
UNUSED(vin);
UNUSED(vout);
/* No velocity feedback - assume that electical velocity is
* velocity set
*/
motor->vel_el = motor->vel.set;
#endif
}
else
#endif
{
/* TODO: velocity observer or sensor */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
/* Store electrical velocity */
motor->vel_el = motor->vel_obs;
#else
ASSERT(0);
#endif
}
#ifdef CONFIG_EXAMPLES_FOC_SENSORED
/* Store filtered velocity */
LP_FILTER(motor->vel.now, motor->vel_el, motor->vel_filter);
/* Get mechanical velocity */
motor->vel_mech = motor->vel_el * motor->phy.one_by_p;
#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS)
errout:
#endif
return ret;

17
examples/foc/foc_motor_f32.h
View File

@ -102,6 +102,17 @@ struct foc_motor_f32_s
float angle_m; /* Motor mechanical angle */
float angle_el; /* Motor electrical angle */
/* Velocity state *********************************************************/
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
float vel_el; /* Velocity - electrical */
float vel_mech; /* Velocity - mechanical */
float vel_filter; /* Velocity low-pass filter */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
float vel_obs; /* Velocity observer output */
#endif
/* Motor setpoints ********************************************************/
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@ -150,6 +161,12 @@ struct foc_motor_f32_s
foc_angle_f32_t qenco; /* Qenco angle handler */
char qedpath[32]; /* Qenco devpath */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
foc_velocity_f32_t vel_div; /* DIV velocity observer */
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_f32_t vel_pll; /* PLL velocity observer */
#endif
};
/****************************************************************************

3
examples/foc/foc_nxscope.c
View File

@ -239,6 +239,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3)
nxscope_chan_init(&nxs->nxs, i++, "svm3", u.u8, 4, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0);
#endif
if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS)
{

1
examples/foc/foc_nxscope.h
View File

@ -52,6 +52,7 @@
#define FOC_NXSCOPE_DQREF (1 << 14) /* DQ reference */
#define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */
#define FOC_NXSCOPE_SVM3 (1 << 16) /* Space-vector modulation sector */
#define FOC_NXSCOPE_VOBS (1 << 17) /* Output from velocity observer */
/* Max 32-bit */
/****************************************************************************

53
examples/foc/foc_parseargs.c
View File

@ -45,6 +45,11 @@
#define OPT_IIV (SCHAR_MAX + 6)
#define OPT_IIS (SCHAR_MAX + 7)
#define OPT_VOPLLKP (SCHAR_MAX + 7)
#define OPT_VOPLLKI (SCHAR_MAX + 8)
#define OPT_VODIVS (SCHAR_MAX + 9)
#define OPT_VODIVF (SCHAR_MAX + 10)
/****************************************************************************
* Private Data
****************************************************************************/
@ -79,6 +84,14 @@ static struct option g_long_options[] =
{ "irs", required_argument, 0, OPT_IRS },
{ "iiv", required_argument, 0, OPT_IIV },
{ "iis", required_argument, 0, OPT_IIS },
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
{ "vopllkp", required_argument, 0, OPT_VOPLLKP },
{ "vopllki", required_argument, 0, OPT_VOPLLKI },
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
{ "vodivs", required_argument, 0, OPT_VODIVS },
{ "vodivf", required_argument, 0, OPT_VODIVF },
#endif
{ 0, 0, 0, 0 }
};
@ -153,6 +166,18 @@ static void foc_help(void)
PRINTF(" [--iis] ind sec (default: %d)\n",
CONFIG_EXAMPLES_FOC_IDENT_IND_SEC);
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
PRINTF(" [--vopllkp] velobs PLL Kp (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP);
PRINTF(" [--vopllki] velobs PLL Ki (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI);
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
PRINTF(" [--vodivs] velobs DIV samples (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES);
PRINTF(" [--vodivf] velobs DIV filter (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER);
#endif
}
/****************************************************************************
@ -226,6 +251,34 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
case OPT_VOPLLKP:
{
args->cfg.vel_pll_kp = atoi(optarg);
break;
}
case OPT_VOPLLKI:
{
args->cfg.vel_pll_ki = atoi(optarg);
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
case OPT_VODIVS:
{
args->cfg.vel_div_samples = atoi(optarg);
break;
}
case OPT_VODIVF:
{
args->cfg.vel_div_filter = atoi(optarg);
break;
}
#endif
case 't':
{
args->time = atoi(optarg);