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/19 11:33:48 UTC
[nuttx-apps] branch master updated: examples/foc: add phase angle observer support (sensorless mode)
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
The following commit(s) were added to refs/heads/master by this push:
new 1c922f2d2 examples/foc: add phase angle observer support (sensorless mode)
1c922f2d2 is described below
commit 1c922f2d2d9c43012464f052f6d2280acf0c53f3
Author: raiden00pl <ra...@railab.me>
AuthorDate: Tue Aug 15 14:38:02 2023 +0200
examples/foc: add phase angle observer support (sensorless mode)
This app can work now as sensorless ESC.
Also introduce a cmd line option that force open-loop control
which is useful when tuning angle/velocity observers
---
examples/foc/Kconfig | 85 ++++++
examples/foc/foc_cfg.h | 20 +-
examples/foc/foc_float_thr.c | 4 +
examples/foc/foc_main.c | 11 +-
examples/foc/foc_motor_b16.c | 573 +++++++++++++++++++++++++++----------
examples/foc/foc_motor_b16.h | 24 +-
examples/foc/foc_motor_f32.c | 587 ++++++++++++++++++++++++++++----------
examples/foc/foc_motor_f32.h | 24 +-
examples/foc/foc_nxscope.c | 3 +
examples/foc/foc_nxscope.h | 1 +
examples/foc/foc_parseargs.c | 65 ++++-
include/industry/foc/foc_common.h | 9 +
12 files changed, 1099 insertions(+), 307 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 8328d02f8..f48d53db3 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -91,6 +91,7 @@ choice
config EXAMPLES_FOC_SENSORLESS
bool "FOC example sensorless configuration"
+ select EXAMPLES_FOC_ANGOBS
config EXAMPLES_FOC_SENSORED
bool "FOC example sensored configuration"
@@ -239,6 +240,90 @@ endif # EXAMPLES_FOC_VELCTRL_PI
endif # EXAMPLES_FOC_HAVE_VEL
+config EXAMPLES_FOC_ANGOBS
+ bool "FOC example phase angle observer support"
+ default n
+
+choice
+ prompt "FOC angle observer selection"
+ default EXAMPLES_FOC_ANGOBS_NFO
+ depends on EXAMPLES_FOC_ANGOBS
+
+config EXAMPLES_FOC_ANGOBS_SMO
+ bool "FOC angle SMO observer"
+ select INDUSTRY_FOC_ANGLE_OSMO
+
+config EXAMPLES_FOC_ANGOBS_NFO
+ bool "FOC angle NFO observer"
+ select INDUSTRY_FOC_ANGLE_ONFO
+
+endchoice # FOC angle observer
+
+if EXAMPLES_FOC_ANGOBS
+
+config EXAMPLES_FOC_ANGOBS_HYS
+ int "FOC angle observer hysteresis [x1]"
+ default 0
+ ---help---
+ Hysteresis added to the observer to open-loop transition.
+
+config EXAMPLES_FOC_ANGOBS_THR
+ int "FOC angle observer velocity threshold [x1]"
+ default 0
+ ---help---
+ Once the motor reaches this speed, we switch from the open-loop angle
+ to the observer angle.
+
+config EXAMPLES_FOC_ANGOBS_MERGE_RATIO
+ int "FOC angle observer merge ratio"
+ default 50
+ range 0 50
+ ---help---
+ This parameter determines how quickly we make the transition from the
+ open-loop angle to the observer angle after reaching the observer threshold
+ velocity. The smaler the value, the smoother the transition.
+ If set to 0 - smooth transition is disabled.
+
+endif # EXAMPLES_FOC_ANGOBS
+
+if EXAMPLES_FOC_ANGOBS_SMO
+
+config EXAMPLES_FOC_ANGOBS_SMO_KSLIDE
+ int "FOC angle SMO observer Kslide (x1000)"
+ default 0
+ ---help---
+ The Kslide coefficient used in observer is:
+ Kslide = EXAMPLES_FOC_ANGOBS_SMO_KSLIDE/1000
+
+config EXAMPLES_FOC_ANGOBS_SMO_ERRMAX
+ int "FOC angle SMO observer err_max (x1000)"
+ default 0
+ ---help---
+ The err_max coefficient used in observer is:
+ err_max = EXAMPLES_FOC_ANGOBS_SMO_ERRMAX/1000
+
+endif # EXAMPLES_FOC_ANGOBS_SMO
+
+if EXAMPLES_FOC_ANGOBS_NFO
+
+config EXAMPLES_FOC_ANGOBS_NFO_GAIN
+ int "FOC angle NFO observer gain (x1)"
+ default 0
+ ---help---
+ The gain coefficient used in observer at maximum duty cycle.
+
+config EXAMPLES_FOC_ANGOBS_NFO_GAINSLOW
+ int "FOC angle NFO observer gain slow (x1)"
+ default 0
+ ---help---
+ The gain coefficient used in observer at minimum duty cycle.
+
+endif # EXAMPLES_FOC_ANGOBS_NFO
+
+config EXAMPLES_FOC_VELOBS
+ bool "FOC example velocity observer support"
+ default n
+
if EXAMPLES_FOC_VELOBS
choice
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
index 069ad061a..9edb488ad 100644
--- a/examples/foc/foc_cfg.h
+++ b/examples/foc/foc_cfg.h
@@ -27,6 +27,8 @@
#include <nuttx/config.h>
+#include <stdbool.h>
+
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
@@ -204,6 +206,12 @@
#define VEL_CONTROL_PRESCALER (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \
CONFIG_EXAMPLES_FOC_VELCTRL_FREQ)
+/* Open-loop to observer angle merge factor */
+
+#if CONFIG_EXAMPLES_FOC_ANGOBS_MERGE_RATIO > 0
+# define ANGLE_MERGE_FACTOR (CONFIG_EXAMPLES_FOC_ANGOBS_MERGE_RATIO / 100.0f)
+#endif
+
/****************************************************************************
* Public Type Definition
****************************************************************************/
@@ -213,7 +221,12 @@ struct foc_thr_cfg_s
int fmode; /* FOC control mode */
int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- int qparam; /* Open-loop Q setting (x1000) */
+ uint32_t qparam; /* Open-loop Q setting (x1000) */
+ bool ol_force; /* Force open-loop */
+# ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ uint32_t ol_thr; /* Observer vel threshold [x1] */
+ uint32_t ol_hys; /* Observer vel hysteresys [x1] */
+# endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
@@ -255,6 +268,11 @@ struct foc_thr_cfg_s
uint32_t vel_pi_kp; /* Vel controller PI Kp (x1000000) */
uint32_t vel_pi_ki; /* Vel controller PI Ki (x1000000) */
#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+ uint32_t ang_nfo_slow; /* Ang NFO slow gain (x1) */
+ uint32_t ang_nfo_gain; /* Ang NFO gain (x1) */
+#endif
};
#endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 8c5127cc7..be03605bb 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -284,6 +284,10 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
ptr = (FAR float *)&motor->vel_obs;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AOBS)
+ ptr = (FAR float *)&motor->angle_obs;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
+#endif
#ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
nxscope_unlock(&nxs->nxs);
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index e559f2539..071618a29 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -75,7 +75,12 @@ struct args_s g_args =
.fmode = CONFIG_EXAMPLES_FOC_FMODE,
.mmode = CONFIG_EXAMPLES_FOC_MMODE,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- .qparam = CONFIG_EXAMPLES_FOC_OPENLOOP_Q,
+ .qparam = CONFIG_EXAMPLES_FOC_OPENLOOP_Q,
+ .ol_force = false,
+# ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ .ol_hys = CONFIG_EXAMPLES_FOC_ANGOBS_HYS,
+ .ol_thr = CONFIG_EXAMPLES_FOC_ANGOBS_THR,
+# endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
.foc_pi_kp = CONFIG_EXAMPLES_FOC_IDQ_KP,
@@ -123,6 +128,10 @@ struct args_s g_args =
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
.vel_pi_kp = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP,
.vel_pi_ki = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI,
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+ .ang_nfo_slow = CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAINSLOW,
+ .ang_nfo_gain = CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAIN,
#endif
}
};
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 8e93da175..1ceda71a3 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -269,7 +269,7 @@ static int foc_runmode_init(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- motor->openloop_now = true;
+ motor->openloop_now = FOC_OPENLOOP_ENABLED;
# else
# error
# endif
@@ -771,6 +771,89 @@ static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor)
return ret;
}
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+/****************************************************************************
+ * Name: foc_motor_openloop_trans
+ ****************************************************************************/
+
+static void foc_motor_openloop_trans(FAR struct foc_motor_b16_s *motor)
+{
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* Set the intergral part of the velocity PI controller equal to the
+ * open-loop Q current value.
+ *
+ * REVISIT: this may casue a velocity overshoot when going from open-loop
+ * to closed-loop. We can either use part of the open-loop Q
+ * current here or gradually reduce the Q current during
+ * transition.
+ */
+
+ motor->vel_pi.part[1] = b16mulb16(motor->dir, motor->openloop_q);
+ motor->vel_pi.part[0] = 0;
+#endif
+}
+
+/****************************************************************************
+ * Name: foc_motor_openloop_angobs
+ ****************************************************************************/
+
+static void foc_motor_openloop_angobs(FAR struct foc_motor_b16_s *motor)
+{
+ b16_t vel_abs = 0;
+
+ vel_abs = b16abs(motor->vel_el);
+
+ /* Disable open-loop if velocity above threshold */
+
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
+ {
+ if (vel_abs >= motor->ol_thr)
+ {
+ /* Store angle error between the forced open-loop angle and
+ * observer output. The error will be gradually eliminated over
+ * the next controller cycles.
+ */
+
+#ifdef ANGLE_MERGE_FACTOR
+ motor->angle_step = b16mulb16(motor->angle_err,
+ ftob16(ANGLE_MERGE_FACTOR));
+ motor->angle_err = motor->angle_ol - motor->angle_obs;
+#endif
+
+ motor->openloop_now = FOC_OPENLOOP_TRANSITION;
+ }
+ }
+
+ /* Handle transition end */
+
+ else if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
+ {
+ if (motor->angle_err == 0)
+ {
+ /* Call open-open loop transition handler */
+
+ foc_motor_openloop_trans(motor);
+
+ motor->openloop_now = FOC_OPENLOOP_DISABLED;
+ }
+ }
+
+ /* Enable open-loop if velocity below threshold with hysteresis */
+
+ else if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
+ {
+ /* For better stability we add hysteresis from transition
+ * from closed-loop to open-loop.
+ */
+
+ if (vel_abs < (motor->ol_thr - motor->ol_hys))
+ {
+ motor->openloop_now = FOC_OPENLOOP_ENABLED;
+ }
+ }
+}
+#endif
+
/****************************************************************************
* Name: foc_motor_run
****************************************************************************/
@@ -786,6 +869,15 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(motor);
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ if (motor->envp->cfg->ol_force == false)
+ {
+ /* Handle open-loop to observer transition */
+
+ foc_motor_openloop_angobs(motor);
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Open-loop works only in velocity control mode */
@@ -883,13 +975,14 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */
- if (motor->openloop_now == true)
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED ||
+ motor->openloop_now == FOC_OPENLOOP_TRANSITION)
{
- /* Get open-loop currents
- * NOTE: Id always set to 0
+ /* Get open-loop currents. Positive for CW direction, negative for
+ * CCW direction. Id always set to 0.
*/
- q_ref = b16idiv(motor->envp->cfg->qparam, 1000);
+ q_ref = b16mulb16(motor->dir, motor->openloop_q);
d_ref = 0;
}
#endif
@@ -916,6 +1009,240 @@ errout:
}
#endif
+/****************************************************************************
+ * Name: foc_motor_ang_get
+ ****************************************************************************/
+
+static int foc_motor_ang_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;
+
+ DEBUGASSERT(motor);
+
+ /* Update open-loop angle handler */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ ain.vel = motor->vel.set;
+#endif
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now != FOC_OPENLOOP_DISABLED)
+ {
+ foc_angle_run_b16(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
+
+ 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)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ ret = foc_angle_run_b16(&motor->ang_smo, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+
+ motor->angle_obs = aout.angle;
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ ret = foc_angle_run_b16(&motor->ang_nfo, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+
+ motor->angle_obs = aout.angle;
+#endif
+
+ /* Store electrical angle from sensor or observer */
+
+ if (aout.type == FOC_ANGLE_TYPE_ELE)
+ {
+ /* Store electrical angle */
+
+ motor->angle_el = aout.angle;
+ }
+
+ else if (aout.type == FOC_ANGLE_TYPE_MECH)
+ {
+ /* Store mechanical angle */
+
+ motor->angle_m = aout.angle;
+
+ /* Convert mechanical angle to electrical angle */
+
+ motor->angle_el = (b16muli(motor->angle_m,
+ motor->phy.p) %
+ MOTOR_ANGLE_E_MAX_B16);
+ }
+
+ else
+ {
+ ASSERT(0);
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Get open-loop phase angle */
+
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
+ {
+ motor->angle_now = motor->angle_ol;
+ motor->angle_el = motor->angle_ol;
+ }
+ else
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
+ /* Handle smooth open-loop to closed-loop transition */
+
+ if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
+ {
+#ifdef ANGLE_MERGE_FACTOR
+ if (b16abs(motor->angle_err) > b16abs(motor->angle_step))
+ {
+ motor->angle_now = motor->angle_obs + motor->angle_err;
+
+ /* Update error */
+
+ motor->angle_err -= motor->angle_step;
+ }
+ else
+#endif
+ {
+ motor->angle_now = motor->angle_obs;
+ motor->angle_err = 0;
+ }
+ }
+
+ /* Get angle from observer if closed-loop now */
+
+ else
+ {
+ motor->angle_now = motor->angle_obs;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
+ /* Get phase angle from sensor */
+
+ motor->angle_now = motor->angle_el;
+#endif
+
+#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_ANGOBS)
+ errout:
+#endif
+ return ret;
+}
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+/****************************************************************************
+ * Name: foc_motor_vel_get
+ ****************************************************************************/
+
+static int foc_motor_vel_get(FAR struct foc_motor_b16_s *motor)
+{
+ struct foc_velocity_in_b16_s vin;
+ struct foc_velocity_out_b16_s vout;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* 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 from observer */
+
+#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 */
+
+#if defined(CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP) && \
+ !defined(CONFIG_EXAMPLES_FOC_VELOBS)
+ /* No velocity feedback - assume that electical velocity is velocity set
+ * in a open-loop contorller.
+ */
+
+ UNUSED(vin);
+ UNUSED(vout);
+
+ motor->vel_el = motor->vel.set;
+#elif defined(CONFIG_EXAMPLES_FOC_VELOBS)
+ if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
+ {
+ /* Get electrical velocity from observer if we are in closed-loop */
+
+ motor->vel_el = motor->vel_obs;
+ }
+ else
+ {
+ /* Otherwise use open-loop velocity */
+
+ motor->vel_el = motor->vel.set;
+ }
+#else
+ /* Need electrical velocity source here - raise assertion */
+
+ ASSERT(0);
+#endif
+
+ LP_FILTER_B16(motor->vel.now, motor->vel_el, motor->vel_filter);
+
+ /* Get mechanical velocity (rad/s) */
+
+ motor->vel_mech = b16mulb16(motor->vel_el, motor->phy.one_by_p);
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+errout:
+#endif
+ return ret;
+}
+#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -936,6 +1263,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_ANGOBS_SMO
+ struct foc_angle_osmo_cfg_b16_s smo_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ struct foc_angle_onfo_cfg_b16_s nfo_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_b16_cfg_s odiv_cfg;
#endif
@@ -965,6 +1298,10 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ motor->ol_thr = ftob16(motor->envp->cfg->ol_thr / 1.0f);
+ motor->ol_hys = ftob16(motor->envp->cfg->ol_hys / 1.0f);
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */
@@ -991,6 +1328,10 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
ol_cfg.per = motor->per;
foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
+
+ /* Store open-loop Q-param */
+
+ motor->openloop_q = ftob16(motor->envp->cfg->qparam / 1000.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
@@ -1055,6 +1396,58 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ /* Initialzie SMO angle observer handler */
+
+ ret = foc_angle_init_b16(&motor->ang_smo,
+ &g_foc_angle_osmo_b16);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Configure SMO angle handler */
+
+ smo_cfg.per = motor->per;
+ smo_cfg.k_slide = ftob16(CONFIG_EXAMPLES_FOC_ANGOBS_SMO_KSLIDE / 1000.0f);
+ smo_cfg.err_max = ftob16(CONFIG_EXAMPLES_FOC_ANGOBS_SMO_ERRMAX / 1000.0f);
+ memcpy(&smo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_b16_s));
+
+ ret = foc_angle_cfg_b16(&motor->ang_smo, &smo_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ /* Initialzie NFO angle observer handler */
+
+ ret = foc_angle_init_b16(&motor->ang_nfo,
+ &g_foc_angle_onfo_b16);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Configure NFO angle handler */
+
+ nfo_cfg.per = motor->per;
+ nfo_cfg.gain = ftob16(motor->envp->cfg->ang_nfo_gain / 1.0f);
+ nfo_cfg.gain_slow = ftob16(motor->envp->cfg->ang_nfo_slow / 1.0f);
+ memcpy(&nfo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_b16_s));
+
+ ret = foc_angle_cfg_b16(&motor->ang_nfo, &nfo_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */
@@ -1142,6 +1535,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
/* Connect align callbacks private data */
+# ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
+ align_cfg.cb.priv = &motor->openloop;
+# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco;
# endif
@@ -1268,6 +1664,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ /* Deinitialize SMO observer handler */
+
+ ret = foc_angle_deinit_b16(&motor->ang_smo);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_deinit_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ /* Deinitialize NFO observer handler */
+
+ ret = foc_angle_deinit_b16(&motor->ang_nfo);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_deinit_b16 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */
@@ -1346,164 +1764,29 @@ 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;
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
- struct foc_velocity_in_b16_s vin;
- struct foc_velocity_out_b16_s vout;
-#endif
- int ret = OK;
+ int ret = OK;
DEBUGASSERT(motor);
- /* Update open-loop angle handler */
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
- ain.vel = motor->vel.set;
-#endif
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_b16(&motor->openloop, &ain, &aout);
-
- /* Store open-loop angle */
-
- 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
+ /* Get motor angle */
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
- ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
+ ret = foc_motor_ang_get(motor);
if (ret < 0)
{
- PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_SENSORED
- /* Handle angle from sensor */
-
- if (aout.type == FOC_ANGLE_TYPE_ELE)
- {
- /* Store electrical angle */
-
- motor->angle_el = aout.angle;
- }
-
- else if (aout.type == FOC_ANGLE_TYPE_MECH)
- {
- /* Store mechanical angle */
-
- motor->angle_m = aout.angle;
-
- /* Convert mechanical angle to electrical angle */
-
- motor->angle_el = (b16muli(motor->angle_m,
- motor->phy.p) %
- MOTOR_ANGLE_E_MAX_B16);
- }
-
- else
- {
- ASSERT(0);
- }
-#endif /* CONFIG_EXAMPLES_FOC_SENSORED */
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Get phase angle now */
-
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
- }
- else
-#endif
- {
- /* Get phase angle from observer or sensor */
-
- motor->angle_now = motor->angle_el;
- }
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
- /* Update velocity handler input data */
+ /* Get motor velocity */
- 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);
+ ret = foc_motor_vel_get(motor);
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)
- {
-#ifdef CONFIG_EXAMPLES_FOC_VELOBS
- /* Electrical velocity from observer */
-
- 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
- {
-#ifdef CONFIG_EXAMPLES_FOC_VELOBS
- /* Store electrical velocity */
-
- motor->vel_el = motor->vel_obs;
-#else
- ASSERT(0);
-#endif
- }
-
- 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 a10bf2d93..3ae905ceb 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -92,6 +92,11 @@ 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 */
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ b16_t ol_thr; /* Angle observer threshold velocity */
+ b16_t ol_hys; /* Angle observer hysteresis */
+ b16_t angle_step; /* Open-loop transition step */
+#endif
/* Velocity controller data ***********************************************/
@@ -100,11 +105,18 @@ struct foc_motor_b16_s
pid_controller_b16_t vel_pi; /* Velocity controller */
#endif
- /* Motor state ************************************************************/
+ /* Angle state ************************************************************/
b16_t angle_now; /* Phase angle now */
b16_t angle_m; /* Motor mechanical angle */
b16_t angle_el; /* Motor electrical angle */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ b16_t angle_ol; /* Phase angle open-loop */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ b16_t angle_obs; /* Angle observer output */
+ b16_t angle_err; /* Open-loop to observer error */
+#endif
/* Velocity state *********************************************************/
@@ -154,8 +166,8 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc_angle_b16_t openloop; /* Open-loop angle handler */
- bool openloop_now; /* Open-loop now */
- b16_t angle_ol; /* Phase angle open-loop */
+ uint8_t openloop_now; /* Open-loop now */
+ b16_t openloop_q; /* Open-loop Q parameter */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
foc_angle_b16_t hall; /* Hall angle handler */
@@ -171,6 +183,12 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_b16_t vel_pll; /* PLL velocity observer */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ foc_angle_b16_t ang_smo; /* SMO angle observer */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ foc_angle_b16_t ang_nfo; /* NFO angle observer */
+#endif
};
/****************************************************************************
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index a3e0b2a72..3c7536345 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -269,7 +269,7 @@ static int foc_runmode_init(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- motor->openloop_now = true;
+ motor->openloop_now = FOC_OPENLOOP_ENABLED;
# else
# error
# endif
@@ -755,6 +755,88 @@ static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
return ret;
}
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+/****************************************************************************
+ * Name: foc_motor_openloop_trans
+ ****************************************************************************/
+
+static void foc_motor_openloop_trans(FAR struct foc_motor_f32_s *motor)
+{
+#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
+ /* Set the intergral part of the velocity PI controller equal to the
+ * open-loop Q current value.
+ *
+ * REVISIT: this may casue a velocity overshoot when going from open-loop
+ * to closed-loop. We can either use part of the open-loop Q
+ * current here or gradually reduce the Q current during
+ * transition.
+ */
+
+ motor->vel_pi.part[1] = motor->dir * motor->openloop_q;
+ motor->vel_pi.part[0] = 0.0f;
+#endif
+}
+
+/****************************************************************************
+ * Name: foc_motor_openloop_angobs
+ ****************************************************************************/
+
+static void foc_motor_openloop_angobs(FAR struct foc_motor_f32_s *motor)
+{
+ float vel_abs = 0.0f;
+
+ vel_abs = fabsf(motor->vel_el);
+
+ /* Disable open-loop if velocity above threshold */
+
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
+ {
+ if (vel_abs >= motor->ol_thr)
+ {
+ /* Store angle error between the forced open-loop angle and
+ * observer output. The error will be gradually eliminated over
+ * the next controller cycles.
+ */
+
+#ifdef ANGLE_MERGE_FACTOR
+ motor->angle_err = motor->angle_ol - motor->angle_obs;
+ motor->angle_step = (motor->angle_err * ANGLE_MERGE_FACTOR);
+#endif
+
+ motor->openloop_now = FOC_OPENLOOP_TRANSITION;
+ }
+ }
+
+ /* Handle transition end */
+
+ else if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
+ {
+ if (motor->angle_err == 0.0f)
+ {
+ /* Call open-open loop transition handler */
+
+ foc_motor_openloop_trans(motor);
+
+ motor->openloop_now = FOC_OPENLOOP_DISABLED;
+ }
+ }
+
+ /* Enable open-loop if velocity below threshold with hysteresis */
+
+ else if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
+ {
+ /* For better stability we add hysteresis from transition
+ * from closed-loop to open-loop.
+ */
+
+ if (vel_abs < (motor->ol_thr - motor->ol_hys))
+ {
+ motor->openloop_now = FOC_OPENLOOP_ENABLED;
+ }
+ }
+}
+#endif
+
/****************************************************************************
* Name: foc_motor_run
****************************************************************************/
@@ -770,10 +852,19 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(motor);
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ if (motor->envp->cfg->ol_force == false)
+ {
+ /* Handle open-loop to observer transition */
+
+ foc_motor_openloop_angobs(motor);
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Open-loop works only in velocity control mode */
- if (motor->openloop_now == true)
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
{
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->cfg->mmode != FOC_MMODE_VEL)
@@ -835,7 +926,7 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
/* Run velocity controller if no in open-loop */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- if (motor->openloop_now == false)
+ if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
#endif
{
/* Get velocity error */
@@ -867,13 +958,14 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */
- if (motor->openloop_now == true)
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED ||
+ motor->openloop_now == FOC_OPENLOOP_TRANSITION)
{
- /* Get open-loop currents
- * NOTE: Id always set to 0
+ /* Get open-loop currents. Positive for CW direction, negative for
+ * CCW direction. Id always set to 0.
*/
- q_ref = (motor->envp->cfg->qparam / 1000.0f);
+ q_ref = motor->dir * motor->openloop_q;
d_ref = 0.0f;
}
#endif
@@ -900,6 +992,246 @@ errout:
}
#endif
+/****************************************************************************
+ * Name: foc_motor_ang_get
+ ****************************************************************************/
+
+static int foc_motor_ang_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;
+
+ DEBUGASSERT(motor);
+
+ /* Update open-loop angle handler */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+ ain.vel = motor->vel.set;
+#endif
+ ain.state = &motor->foc_state;
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ if (motor->openloop_now != FOC_OPENLOOP_DISABLED)
+ {
+ foc_angle_run_f32(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
+
+ 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)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ ret = foc_angle_run_f32(&motor->ang_smo, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+
+ motor->angle_obs = aout.angle;
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ ret = foc_angle_run_f32(&motor->ang_nfo, &ain, &aout);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+ goto errout;
+ }
+
+ motor->angle_obs = aout.angle;
+#endif
+
+ /* Store electrical angle from sensor or observer */
+
+ if (aout.type == FOC_ANGLE_TYPE_ELE)
+ {
+ /* Store as electrical angle */
+
+ motor->angle_el = aout.angle;
+ }
+
+ else if (aout.type == FOC_ANGLE_TYPE_MECH)
+ {
+ /* Store as mechanical angle */
+
+ motor->angle_m = aout.angle;
+
+ /* Convert mechanical angle to electrical angle */
+
+ motor->angle_el = fmodf(motor->angle_m * motor->phy.p,
+ MOTOR_ANGLE_E_MAX);
+ }
+
+ else
+ {
+ ASSERT(0);
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ /* Get open-loop phase angle */
+
+ if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
+ {
+ motor->angle_now = motor->angle_ol;
+ motor->angle_el = motor->angle_ol;
+ }
+ else
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
+ /* Handle smooth open-loop to closed-loop transition */
+
+ if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
+ {
+#ifdef ANGLE_MERGE_FACTOR
+ if (fabsf(motor->angle_err) > fabsf(motor->angle_step))
+ {
+ motor->angle_now = motor->angle_obs + motor->angle_err;
+
+ /* Update error */
+
+ motor->angle_err -= motor->angle_step;
+ }
+ else
+#endif
+ {
+ motor->angle_now = motor->angle_obs;
+ motor->angle_err = 0.0f;
+ }
+ }
+
+ /* Get angle from observer if closed-loop now */
+
+ else
+ {
+ motor->angle_now = motor->angle_obs;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
+ /* Get phase angle from sensor */
+
+ motor->angle_now = motor->angle_el;
+#endif
+
+#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_ANGOBS)
+ errout:
+#endif
+ return ret;
+}
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+/****************************************************************************
+ * Name: foc_motor_vel_get
+ ****************************************************************************/
+
+static int foc_motor_vel_get(FAR struct foc_motor_f32_s *motor)
+{
+ struct foc_velocity_in_f32_s vin;
+ struct foc_velocity_out_f32_s vout;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* 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 */
+
+#if defined(CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP) && \
+ !defined(CONFIG_EXAMPLES_FOC_VELOBS)
+ /* No velocity feedback - assume that electical velocity is
+ * velocity set
+ */
+
+ UNUSED(vin);
+ UNUSED(vout);
+
+ motor->vel_el = motor->vel.set;
+#elif defined(CONFIG_EXAMPLES_FOC_VELOBS)
+ if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
+ {
+ /* Get electrical velocity from observer if we are in closed-loop */
+
+ motor->vel_el = motor->vel_obs;
+ }
+ else
+ {
+ /* Otherwise use open-loop velocity */
+
+ motor->vel_el = motor->vel.set;
+ }
+#else
+ /* Need electrical velocity source here - raise assertion */
+
+ ASSERT(0);
+#endif
+
+ /* Store filtered velocity */
+
+ LP_FILTER(motor->vel.now, motor->vel_el, motor->vel_filter);
+
+ /* Get mechanical velocity (rad/s) */
+
+ motor->vel_mech = motor->vel_el * motor->phy.one_by_p;
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+errout:
+#endif
+ return ret;
+}
+#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -920,6 +1252,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_ANGOBS_SMO
+ struct foc_angle_osmo_cfg_f32_s smo_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ struct foc_angle_onfo_cfg_f32_s nfo_cfg;
+#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_f32_cfg_s odiv_cfg;
#endif
@@ -949,6 +1287,10 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ motor->ol_thr = (motor->envp->cfg->ol_thr / 1.0f);
+ motor->ol_hys = (motor->envp->cfg->ol_hys / 1.0f);
+#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */
@@ -975,6 +1317,10 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
ol_cfg.per = motor->per;
foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
+
+ /* Store open-loop Q-param */
+
+ motor->openloop_q = motor->envp->cfg->qparam / 1000.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
@@ -1039,6 +1385,58 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ /* Initialzie SMO angle observer handler */
+
+ ret = foc_angle_init_f32(&motor->ang_smo,
+ &g_foc_angle_osmo_f32);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Configure SMO angle handler */
+
+ smo_cfg.per = motor->per;
+ smo_cfg.k_slide = (CONFIG_EXAMPLES_FOC_ANGOBS_SMO_KSLIDE / 1000.0f);
+ smo_cfg.err_max = (CONFIG_EXAMPLES_FOC_ANGOBS_SMO_ERRMAX / 1000.0f);
+ memcpy(&smo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_f32_s));
+
+ ret = foc_angle_cfg_f32(&motor->ang_smo, &smo_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ /* Initialzie NFO angle observer handler */
+
+ ret = foc_angle_init_f32(&motor->ang_nfo,
+ &g_foc_angle_onfo_f32);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Configure NFO angle handler */
+
+ nfo_cfg.per = motor->per;
+ nfo_cfg.gain = (motor->envp->cfg->ang_nfo_gain / 1.0f);
+ nfo_cfg.gain_slow = (motor->envp->cfg->ang_nfo_slow / 1.0f);
+ memcpy(&nfo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_f32_s));
+
+ ret = foc_angle_cfg_f32(&motor->ang_nfo, &nfo_cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */
@@ -1126,6 +1524,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
/* Connect align callbacks private data */
+# ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
+ align_cfg.cb.priv = &motor->openloop;
+# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco;
# endif
@@ -1254,6 +1655,28 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ /* Deinitialize SMO observer handler */
+
+ ret = foc_angle_deinit_f32(&motor->ang_smo);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ /* Deinitialize NFO observer handler */
+
+ ret = foc_angle_deinit_f32(&motor->ang_nfo);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
+ goto errout;
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */
@@ -1332,169 +1755,29 @@ 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;
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
- struct foc_velocity_in_f32_s vin;
- struct foc_velocity_out_f32_s vout;
-#endif
- int ret = OK;
+ int ret = OK;
DEBUGASSERT(motor);
- /* Update open-loop angle handler */
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
- ain.vel = motor->vel.set;
-#endif
- ain.angle = motor->angle_now;
- ain.dir = motor->dir;
+ /* Get motor angle */
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- foc_angle_run_f32(&motor->openloop, &ain, &aout);
-
- /* Store open-loop angle */
-
- motor->angle_ol = aout.angle;
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
- ret = foc_angle_run_f32(&motor->qenco, &ain, &aout);
+ ret = foc_motor_ang_get(motor);
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)
- {
- PRINTF("ERROR: foc_angle_run failed %d\n", ret);
- goto errout;
- }
-#endif
-
-#ifdef CONFIG_EXAMPLES_FOC_SENSORED
- /* Handle angle from sensor */
-
- if (aout.type == FOC_ANGLE_TYPE_ELE)
- {
- /* Store electrical angle */
-
- motor->angle_el = aout.angle;
- }
-
- else if (aout.type == FOC_ANGLE_TYPE_MECH)
- {
- /* Store mechanical angle */
-
- motor->angle_m = aout.angle;
-
- /* Convert mechanical angle to electrical angle */
-
- motor->angle_el = fmodf(motor->angle_m * motor->phy.p,
- MOTOR_ANGLE_E_MAX);
- }
-
- else
- {
- ASSERT(0);
- }
-#endif /* CONFIG_EXAMPLES_FOC_SENSORED */
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
- /* Get phase angle now */
-
- if (motor->openloop_now == true)
- {
- motor->angle_now = motor->angle_ol;
- }
- else
-#endif
- {
- /* Get phase angle from observer or sensor */
-
- 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 motor velocity */
- /* 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);
+ ret = foc_motor_vel_get(motor);
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)
- {
-#ifdef CONFIG_EXAMPLES_FOC_VELOBS
- /* Electrical velocity from observer */
-
- 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
- {
-#ifdef CONFIG_EXAMPLES_FOC_VELOBS
- /* Store electrical velocity */
-
- motor->vel_el = motor->vel_obs;
-#else
- ASSERT(0);
#endif
- }
- /* 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 ddcec9fc9..71bdd36ff 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -92,6 +92,10 @@ 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 */
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ float ol_thr; /* Angle observer threshold velocity */
+ float ol_hys; /* Angle observer hysteresis */
+#endif
/* Velocity controller data ***********************************************/
@@ -100,11 +104,19 @@ struct foc_motor_f32_s
pid_controller_f32_t vel_pi; /* Velocity controller */
#endif
- /* Motor state ************************************************************/
+ /* Angle state ************************************************************/
float angle_now; /* Phase angle now */
float angle_m; /* Motor mechanical angle */
float angle_el; /* Motor electrical angle */
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+ float angle_ol; /* Phase angle open-loop */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ float angle_obs; /* Angle observer output */
+ float angle_err; /* Open-loop to observer error */
+ float angle_step; /* Open-loop transition step */
+#endif
/* Velocity state *********************************************************/
@@ -154,8 +166,8 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc_angle_f32_t openloop; /* Open-loop angle handler */
- bool openloop_now; /* Open-loop now */
- float angle_ol; /* Phase angle open-loop */
+ uint8_t openloop_now; /* Open-loop now */
+ float openloop_q; /* Open-loop Q parameter */
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
foc_angle_f32_t hall; /* Hall angle handler */
@@ -171,6 +183,12 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_f32_t vel_pll; /* PLL velocity observer */
#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
+ foc_angle_f32_t ang_smo; /* SMO angle observer */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
+ foc_angle_f32_t ang_nfo; /* NFO angle observer */
+#endif
};
/****************************************************************************
diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c
index 101a07ac7..8ca2b3499 100644
--- a/examples/foc/foc_nxscope.c
+++ b/examples/foc/foc_nxscope.c
@@ -242,6 +242,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0);
#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AOBS)
+ nxscope_chan_init(&nxs->nxs, i++, "aobs", 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 38a998e2a..911e0afdf 100644
--- a/examples/foc/foc_nxscope.h
+++ b/examples/foc/foc_nxscope.h
@@ -53,6 +53,7 @@
#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 */
+#define FOC_NXSCOPE_AOBS (1 << 18) /* Output from angle observer */
/* Max 32-bit */
/****************************************************************************
diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c
index b43812164..e8eddd47a 100644
--- a/examples/foc/foc_parseargs.c
+++ b/examples/foc/foc_parseargs.c
@@ -53,6 +53,13 @@
#define OPT_VCPIKP (SCHAR_MAX + 11)
#define OPT_VCPIKI (SCHAR_MAX + 12)
+#define OPT_ANFOS (SCHAR_MAX + 13)
+#define OPT_ANFOG (SCHAR_MAX + 14)
+
+#define OPT_OLFORCE (SCHAR_MAX + 16)
+#define OPT_OLTHR (SCHAR_MAX + 17)
+#define OPT_OLHYS (SCHAR_MAX + 18)
+
/****************************************************************************
* Private Data
****************************************************************************/
@@ -76,6 +83,11 @@ static struct option g_long_options[] =
{ "en", required_argument, 0, 'j' },
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
{ "oqset", required_argument, 0, 'o' },
+ { "olforce", no_argument, 0, OPT_OLFORCE },
+# ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ { "olthr", required_argument, 0, OPT_OLTHR },
+ { "olhys", required_argument, 0, OPT_OLHYS },
+# endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
{ "fkp", required_argument, 0, OPT_FKP },
@@ -99,6 +111,10 @@ static struct option g_long_options[] =
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
{ "vcpikp", required_argument, 0, OPT_VCPIKP },
{ "vcpiki", required_argument, 0, OPT_VCPIKI },
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+ { "anfos", required_argument, 0, OPT_ANFOS },
+ { "anfog", required_argument, 0, OPT_ANFOG },
#endif
{ 0, 0, 0, 0 }
};
@@ -154,6 +170,13 @@ static void foc_help(void)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
PRINTF(" [-o] openloop Vq/Iq setting [x1000] (default: %d)\n",
CONFIG_EXAMPLES_FOC_OPENLOOP_Q);
+ PRINTF(" [--olforce] force openloop\n");
+# ifdef CONFIG_EXAMPLES_FOC_ANGOBS
+ PRINTF(" [--olthr] observer vel threshold [x1] (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_ANGOBS_THR);
+ PRINTF(" [--olhys] observer vel hysteresys [x1] (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_ANGOBS_THR);
+# endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
PRINTF(" [--fki] PI Kp coefficient [x1000] (default: %d)\n",
@@ -186,11 +209,17 @@ static void foc_help(void)
CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER);
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
- PRINTF(" [--vpikp] velctrl PI Kp (default: %d)\n",
+ PRINTF(" [--vcpikp] velctrl PI Kp (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP);
- PRINTF(" [--vpiki] velctrl PI Ki (default: %d)\n",
+ PRINTF(" [--vcpiki] velctrl PI Ki (default: %d)\n",
CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI);
#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+ PRINTF(" [--anfos] angobs NFO Slow (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAINSLOW);
+ PRINTF(" [--anfog] angobs NFO Gain (default: %d)\n",
+ CONFIG_EXAMPLES_FOC_ANGOBS_NFO_GAIN);
+#endif
}
/****************************************************************************
@@ -306,6 +335,20 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
}
#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+ case OPT_ANFOS:
+ {
+ args->cfg.ang_nfo_slow = atoi(optarg);
+ break;
+ }
+
+ case OPT_ANFOG:
+ {
+ args->cfg.ang_nfo_gain = atoi(optarg);
+ break;
+ }
+#endif
+
case 't':
{
args->time = atoi(optarg);
@@ -372,6 +415,24 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
args->cfg.qparam = atoi(optarg);
break;
}
+
+ case OPT_OLFORCE:
+ {
+ args->cfg.ol_force = true;
+ break;
+ }
+
+ case OPT_OLTHR:
+ {
+ args->cfg.ol_thr = atoi(optarg);
+ break;
+ }
+
+ case OPT_OLHYS:
+ {
+ args->cfg.ol_hys = atoi(optarg);
+ break;
+ }
#endif
case '?':
diff --git a/include/industry/foc/foc_common.h b/include/industry/foc/foc_common.h
index fa50f5dab..4530647ca 100644
--- a/include/industry/foc/foc_common.h
+++ b/include/industry/foc/foc_common.h
@@ -80,6 +80,15 @@ enum foc_angle_type_e
FOC_ANGLE_TYPE_MECH = 2, /* Mechanical angle */
};
+/* Open-loop stage */
+
+enum foc_openloop_stage_e
+{
+ FOC_OPENLOOP_ENABLED = 1, /* Open-loop enabled */
+ FOC_OPENLOOP_TRANSITION = 2, /* Open-loop to closed-loop transition */
+ FOC_OPENLOOP_DISABLED = 3, /* Open-loop disabled */
+};
+
/****************************************************************************
* Public Function Prototypes
****************************************************************************/