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

examples/foc: add phase angle observer support (sensorless mode)

Browse Source 
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
This commit is contained in:
raiden00pl 2023-08-15 14:38:02 +02:00 committed by Xiang Xiao
parent a4ceb82d67
commit 1c922f2d2d
12 changed files with 1099 additions and 307 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
examples/foc/Kconfig
View File

@ -91,6 +91,7 @@ choice
config EXAMPLES_FOC_SENSORLESS config EXAMPLES_FOC_SENSORLESS
bool "FOC example sensorless configuration" bool "FOC example sensorless configuration"
select EXAMPLES_FOC_ANGOBS
config EXAMPLES_FOC_SENSORED config EXAMPLES_FOC_SENSORED
bool "FOC example sensored configuration" bool "FOC example sensored configuration"
@ -239,6 +240,90 @@ endif # EXAMPLES_FOC_VELCTRL_PI
endif # EXAMPLES_FOC_HAVE_VEL 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 if EXAMPLES_FOC_VELOBS
choice choice

20
examples/foc/foc_cfg.h
View File

@ -27,6 +27,8 @@
#include <nuttx/config.h> #include <nuttx/config.h>
#include <stdbool.h>
/**************************************************************************** /****************************************************************************
* Pre-processor Definitions * Pre-processor Definitions
****************************************************************************/ ****************************************************************************/
@ -204,6 +206,12 @@
#define VEL_CONTROL_PRESCALER (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \ #define VEL_CONTROL_PRESCALER (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \
CONFIG_EXAMPLES_FOC_VELCTRL_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 * Public Type Definition
****************************************************************************/ ****************************************************************************/
@ -213,7 +221,12 @@ struct foc_thr_cfg_s
int fmode; /* FOC control mode */ int fmode; /* FOC control mode */
int mmode; /* Motor control mode */ int mmode; /* Motor control mode */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #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 #endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #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_kp; /* Vel controller PI Kp (x1000000) */
uint32_t vel_pi_ki; /* Vel controller PI Ki (x1000000) */ uint32_t vel_pi_ki; /* Vel controller PI Ki (x1000000) */
#endif #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 */ #endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */

4
examples/foc/foc_float_thr.c
View File

@ -284,6 +284,10 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
ptr = (FAR float *)&motor->vel_obs; ptr = (FAR float *)&motor->vel_obs;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif #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 #ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
nxscope_unlock(&nxs->nxs); nxscope_unlock(&nxs->nxs);

9
examples/foc/foc_main.c
View File

@ -76,6 +76,11 @@ struct args_s g_args =
.mmode = CONFIG_EXAMPLES_FOC_MMODE, .mmode = CONFIG_EXAMPLES_FOC_MMODE,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #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 #endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
.foc_pi_kp = CONFIG_EXAMPLES_FOC_IDQ_KP, .foc_pi_kp = CONFIG_EXAMPLES_FOC_IDQ_KP,
@ -123,6 +128,10 @@ struct args_s g_args =
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI #ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
.vel_pi_kp = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP, .vel_pi_kp = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KP,
.vel_pi_ki = CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI, .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 #endif
} }
}; };

571
examples/foc/foc_motor_b16.c
View File

@ -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_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP # ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
motor->openloop_now = true; motor->openloop_now = FOC_OPENLOOP_ENABLED;
# else # else
# error # error
# endif # endif
@ -771,6 +771,89 @@ static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor)
return ret; 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 * Name: foc_motor_run
****************************************************************************/ ****************************************************************************/
@ -786,6 +869,15 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
DEBUGASSERT(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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Open-loop works only in velocity control mode */ /* 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */ /* 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 /* Get open-loop currents. Positive for CW direction, negative for
* NOTE: Id always set to 0 * 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; d_ref = 0;
} }
#endif #endif
@ -916,6 +1009,240 @@ errout:
} }
#endif #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 * Public Functions
****************************************************************************/ ****************************************************************************/
@ -936,6 +1263,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_b16_s hl_cfg; struct foc_hall_cfg_b16_s hl_cfg;
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_b16_cfg_s odiv_cfg; struct foc_vel_div_b16_cfg_s odiv_cfg;
#endif #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->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */ /* Initialize controller run mode */
@ -991,6 +1328,10 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
ol_cfg.per = motor->per; ol_cfg.per = motor->per;
foc_angle_cfg_b16(&motor->openloop, &ol_cfg); foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
/* Store open-loop Q-param */
motor->openloop_q = ftob16(motor->envp->cfg->qparam / 1000.0f);
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO #ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
@ -1055,6 +1396,58 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
} }
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */ /* Initialize velocity observer */
@ -1142,6 +1535,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
/* Connect align callbacks private data */ /* Connect align callbacks private data */
# ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
align_cfg.cb.priv = &motor->openloop;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO # ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco; align_cfg.cb.priv = &motor->qenco;
# endif # endif
@ -1268,6 +1664,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
} }
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */ /* Deinitialize DIV observer handler */
@ -1346,164 +1764,29 @@ errout:
int foc_motor_get(FAR struct foc_motor_b16_s *motor) 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); DEBUGASSERT(motor);
/* Update open-loop angle handler */ /* Get motor angle */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL ret = foc_motor_ang_get(motor);
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) if (ret < 0)
{ {
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout; 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_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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Update velocity handler input data */ /* Get motor velocity */
vin.state = &motor->foc_state; ret = foc_motor_vel_get(motor);
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) if (ret < 0)
{ {
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout; goto errout;
} }
#endif #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)
{
#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: errout:
#endif
return ret; return ret;
} }

24
examples/foc/foc_motor_b16.h
View File

@ -92,6 +92,11 @@ struct foc_motor_b16_s
b16_t per; /* Controller period in seconds */ b16_t per; /* Controller period in seconds */
b16_t iphase_adc; /* Iphase ADC scaling factor */ b16_t iphase_adc; /* Iphase ADC scaling factor */
b16_t pwm_duty_max; /* PWM duty max */ 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 ***********************************************/ /* Velocity controller data ***********************************************/
@ -100,11 +105,18 @@ struct foc_motor_b16_s
pid_controller_b16_t vel_pi; /* Velocity controller */ pid_controller_b16_t vel_pi; /* Velocity controller */
#endif #endif
/* Motor state ************************************************************/ /* Angle state ************************************************************/
b16_t angle_now; /* Phase angle now */ b16_t angle_now; /* Phase angle now */
b16_t angle_m; /* Motor mechanical angle */ b16_t angle_m; /* Motor mechanical angle */
b16_t angle_el; /* Motor electrical 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 *********************************************************/ /* Velocity state *********************************************************/
@ -154,8 +166,8 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc_angle_b16_t openloop; /* Open-loop angle handler */ foc_angle_b16_t openloop; /* Open-loop angle handler */
bool openloop_now; /* Open-loop now */ uint8_t openloop_now; /* Open-loop now */
b16_t angle_ol; /* Phase angle open-loop */ b16_t openloop_q; /* Open-loop Q parameter */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
foc_angle_b16_t hall; /* Hall angle handler */ foc_angle_b16_t hall; /* Hall angle handler */
@ -171,6 +183,12 @@ struct foc_motor_b16_s
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL #ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_b16_t vel_pll; /* PLL velocity observer */ foc_velocity_b16_t vel_pll; /* PLL velocity observer */
#endif #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
}; };
/**************************************************************************** /****************************************************************************

585
examples/foc/foc_motor_f32.c
View File

@ -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_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP # ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
motor->openloop_now = true; motor->openloop_now = FOC_OPENLOOP_ENABLED;
# else # else
# error # error
# endif # endif
@ -755,6 +755,88 @@ static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
return ret; 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 * Name: foc_motor_run
****************************************************************************/ ****************************************************************************/
@ -770,10 +852,19 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
DEBUGASSERT(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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Open-loop works only in velocity control mode */ /* 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 # ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->cfg->mmode != FOC_MMODE_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 */ /* Run velocity controller if no in open-loop */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
if (motor->openloop_now == false) if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
#endif #endif
{ {
/* Get velocity error */ /* 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */ /* 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 /* Get open-loop currents. Positive for CW direction, negative for
* NOTE: Id always set to 0 * 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; d_ref = 0.0f;
} }
#endif #endif
@ -900,6 +992,246 @@ errout:
} }
#endif #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 * Public Functions
****************************************************************************/ ****************************************************************************/
@ -920,6 +1252,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_f32_s hl_cfg; struct foc_hall_cfg_f32_s hl_cfg;
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_f32_cfg_s odiv_cfg; struct foc_vel_div_f32_cfg_s odiv_cfg;
#endif #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->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */ /* Initialize controller run mode */
@ -975,6 +1317,10 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
ol_cfg.per = motor->per; ol_cfg.per = motor->per;
foc_angle_cfg_f32(&motor->openloop, &ol_cfg); foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
/* Store open-loop Q-param */
motor->openloop_q = motor->envp->cfg->qparam / 1000.0f;
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO #ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
@ -1039,6 +1385,58 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
} }
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */ /* Initialize velocity observer */
@ -1126,6 +1524,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
/* Connect align callbacks private data */ /* Connect align callbacks private data */
# ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
align_cfg.cb.priv = &motor->openloop;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO # ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco; align_cfg.cb.priv = &motor->qenco;
# endif # endif
@ -1254,6 +1655,28 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
} }
#endif #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 #ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */ /* Deinitialize DIV observer handler */
@ -1332,169 +1755,29 @@ errout:
int foc_motor_get(FAR struct foc_motor_f32_s *motor) 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); DEBUGASSERT(motor);
/* Update open-loop angle handler */ /* Get motor angle */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL ret = foc_motor_ang_get(motor);
ain.vel = motor->vel.set;
#endif
ain.angle = motor->angle_now;
ain.dir = motor->dir;
#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);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout; 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 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Update velocity handler input data */ /* Get motor velocity */
vin.state = &motor->foc_state; ret = foc_motor_vel_get(motor);
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) if (ret < 0)
{ {
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout; goto errout;
} }
motor->vel_obs = vout.velocity;
#endif #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)
{
#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: errout:
#endif
return ret; return ret;
} }

24
examples/foc/foc_motor_f32.h
View File

@ -92,6 +92,10 @@ struct foc_motor_f32_s
float per; /* Controller period in seconds */ float per; /* Controller period in seconds */
float iphase_adc; /* Iphase ADC scaling factor */ float iphase_adc; /* Iphase ADC scaling factor */
float pwm_duty_max; /* PWM duty max */ 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 ***********************************************/ /* Velocity controller data ***********************************************/
@ -100,11 +104,19 @@ struct foc_motor_f32_s
pid_controller_f32_t vel_pi; /* Velocity controller */ pid_controller_f32_t vel_pi; /* Velocity controller */
#endif #endif
/* Motor state ************************************************************/ /* Angle state ************************************************************/
float angle_now; /* Phase angle now */ float angle_now; /* Phase angle now */
float angle_m; /* Motor mechanical angle */ float angle_m; /* Motor mechanical angle */
float angle_el; /* Motor electrical 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 *********************************************************/ /* Velocity state *********************************************************/
@ -154,8 +166,8 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc_angle_f32_t openloop; /* Open-loop angle handler */ foc_angle_f32_t openloop; /* Open-loop angle handler */
bool openloop_now; /* Open-loop now */ uint8_t openloop_now; /* Open-loop now */
float angle_ol; /* Phase angle open-loop */ float openloop_q; /* Open-loop Q parameter */
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
foc_angle_f32_t hall; /* Hall angle handler */ foc_angle_f32_t hall; /* Hall angle handler */
@ -171,6 +183,12 @@ struct foc_motor_f32_s
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL #ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
foc_velocity_f32_t vel_pll; /* PLL velocity observer */ foc_velocity_f32_t vel_pll; /* PLL velocity observer */
#endif #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
}; };
/**************************************************************************** /****************************************************************************

3
examples/foc/foc_nxscope.c
View File

@ -242,6 +242,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS) #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0); nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0);
#endif #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) if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS)
{ {

1
examples/foc/foc_nxscope.h
View File

@ -53,6 +53,7 @@
#define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */ #define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */
#define FOC_NXSCOPE_SVM3 (1 << 16) /* Space-vector modulation sector */ #define FOC_NXSCOPE_SVM3 (1 << 16) /* Space-vector modulation sector */
#define FOC_NXSCOPE_VOBS (1 << 17) /* Output from velocity observer */ #define FOC_NXSCOPE_VOBS (1 << 17) /* Output from velocity observer */
#define FOC_NXSCOPE_AOBS (1 << 18) /* Output from angle observer */
/* Max 32-bit */ /* Max 32-bit */
/**************************************************************************** /****************************************************************************

65
examples/foc/foc_parseargs.c
View File

@ -53,6 +53,13 @@
#define OPT_VCPIKP (SCHAR_MAX + 11) #define OPT_VCPIKP (SCHAR_MAX + 11)
#define OPT_VCPIKI (SCHAR_MAX + 12) #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 * Private Data
****************************************************************************/ ****************************************************************************/
@ -76,6 +83,11 @@ static struct option g_long_options[] =
{ "en", required_argument, 0, 'j' }, { "en", required_argument, 0, 'j' },
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
{ "oqset", required_argument, 0, 'o' }, { "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 #endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
{ "fkp", required_argument, 0, OPT_FKP }, { "fkp", required_argument, 0, OPT_FKP },
@ -99,6 +111,10 @@ static struct option g_long_options[] =
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI #ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
{ "vcpikp", required_argument, 0, OPT_VCPIKP }, { "vcpikp", required_argument, 0, OPT_VCPIKP },
{ "vcpiki", required_argument, 0, OPT_VCPIKI }, { "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 #endif
{ 0, 0, 0, 0 } { 0, 0, 0, 0 }
}; };
@ -154,6 +170,13 @@ static void foc_help(void)
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
PRINTF(" [-o] openloop Vq/Iq setting [x1000] (default: %d)\n", PRINTF(" [-o] openloop Vq/Iq setting [x1000] (default: %d)\n",
CONFIG_EXAMPLES_FOC_OPENLOOP_Q); 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 #endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
PRINTF(" [--fki] PI Kp coefficient [x1000] (default: %d)\n", PRINTF(" [--fki] PI Kp coefficient [x1000] (default: %d)\n",
@ -186,11 +209,17 @@ static void foc_help(void)
CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER); CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER);
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI #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); 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); CONFIG_EXAMPLES_FOC_VELCTRL_PI_KI);
#endif #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 #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': case 't':
{ {
args->time = atoi(optarg); 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); args->cfg.qparam = atoi(optarg);
break; 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 #endif
case '?': case '?':

9
include/industry/foc/foc_common.h
View File

@ -80,6 +80,15 @@ enum foc_angle_type_e
FOC_ANGLE_TYPE_MECH = 2, /* Mechanical angle */ 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 * Public Function Prototypes
****************************************************************************/ ****************************************************************************/