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 2023/10/17 14:42:50 UTC
[nuttx-apps] 02/04: examples/foc: add velocity observers
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/nuttx-apps.git
commit 8e0819df04a3f99eea3692c37446b2ed4b9608a8
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Nov 5 11:00:19 2022 +0100
examples/foc: add velocity observers
---
examples/foc/Kconfig | 54 ++++++++++++
examples/foc/foc_cfg.h | 13 +++
examples/foc/foc_fixed16_thr.c | 8 +-
examples/foc/foc_float_thr.c | 12 ++-
examples/foc/foc_main.c | 11 +++
examples/foc/foc_motor_b16.c | 185 ++++++++++++++++++++++++++++++++++++---
examples/foc/foc_motor_b16.h | 17 ++++
examples/foc/foc_motor_f32.c | 191 ++++++++++++++++++++++++++++++++++++++---
examples/foc/foc_motor_f32.h | 17 ++++
examples/foc/foc_nxscope.c | 3 +
examples/foc/foc_nxscope.h | 1 +
examples/foc/foc_parseargs.c | 53 ++++++++++++
12 files changed, 537 insertions(+), 28 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 057dfbc3c..4b829ed84 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -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
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 14a1d1a47..ac2e6b3bc 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -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 */
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 66ce7feab..d3d7bbc5c 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -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;
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index cc362a1da..94d8596be 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -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);
}
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index f4d8f95d2..9258a46f6 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -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
}
};
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index f1b6be344..06a7201d2 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -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;
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 8159cbb2d..843302da8 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -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
};
/****************************************************************************
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index fc4e7f9d2..da6bdcab4 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -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_el = motor->vel_obs;
+#else
+ UNUSED(vin);
+ UNUSED(vout);
- motor->vel.now = motor->vel.set;
+ /* 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;
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index 7dac93ca8..84d8a3c7b 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -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
};
/****************************************************************************
diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c
index a6fbde880..101a07ac7 100644
--- a/examples/foc/foc_nxscope.c
+++ b/examples/foc/foc_nxscope.c
@@ -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)
{
diff --git a/examples/foc/foc_nxscope.h b/examples/foc/foc_nxscope.h
index ef7a79a6d..38a998e2a 100644
--- a/examples/foc/foc_nxscope.h
+++ b/examples/foc/foc_nxscope.h
@@ -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 */
/****************************************************************************
diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c
index fa4de580e..609500eac 100644
--- a/examples/foc/foc_parseargs.c
+++ b/examples/foc/foc_parseargs.c
@@ -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);