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

lib_observer: add nolinear fluxlink observer

Browse Source
This commit is contained in:
zouboan 2022-02-13 13:46:40 +08:00 committed by Alan Carvalho de Assis
parent 8f6be5e4ce
commit 132f27a91a
2 changed files with 217 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
include/dsp.h
View File

@ -293,12 +293,13 @@ struct motor_sobserver_div_f32_s
};
/* Speed observer PLL method data */
#if 0
struct motor_sobserver_pll_f32_s
{
/* TODO */
float pll_phase;
float pll_kp;
float pll_ki;
};
#endif
/* Motor Sliding Mode Observer private data */
@ -320,6 +321,14 @@ struct motor_observer_smo_f32_s
ab_frame_f32_t sign; /* Bang-bang controller sign */
};
/* Motor Nonlinear FluxLink Observer private data */
struct motor_observer_nfo_f32_s
{
float x1;
float x2;
};
/* FOC initialize data */
struct foc_initdata_f32_s
@ -367,6 +376,7 @@ struct foc_data_f32_s
struct motor_phy_params_f32_s
{
uint8_t p; /* Number of the motor pole pairs */
float flux_link; /* Flux linkage */
float res; /* Phase-to-neutral resistance */
float ind; /* Average phase-to-neutral inductance */
float one_by_ind; /* Inverse phase-to-neutral inductance */
@ -535,6 +545,16 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
float angle, float dir);
void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo);
void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
FAR struct motor_phy_params_f32_s *phy, float gain);
void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
float pll_kp, float pll_ki);
void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
float angle, float dir);
/* Motor openloop control */
void motor_openloop_init(FAR struct openloop_data_f32_s *op, float per);

195
libs/libdsp/lib_observer.c
View File

@ -31,6 +31,15 @@
#define ANGLE_DIFF_THR (M_PI_F)
/* nan check for floats */
#define IS_NAN(x) ((x) != (x))
#define NAN_ZERO(x) (x = IS_NAN(x) ? 0.0 : x)
/* Squared */
#define SQ(x) ((x) * (x))
/****************************************************************************
* Public Functions
****************************************************************************/
@ -258,7 +267,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
{
filter = 0.99f;
}
else if (filter <= 0.0f)
else if (filter < 0.005f)
{
filter = 0.005f;
}
@ -495,6 +504,190 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
so->angle_prev = angle;
}
/****************************************************************************
* Name: motor_observer_nfo_init
*
* Description:
* Initialize motor nolinear fluxlink observer.
*
* Input Parameters:
* smo - pointer to the nolinear fluxlink observer private data
* kslide - SMO gain
* err_max - linear region upper limit
*
* Returned Value:
* None
*
****************************************************************************/
void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo)
{
LIBDSP_DEBUGASSERT(smo != NULL);
/* Reset structure */
memset(nfo, 0, sizeof(struct motor_observer_nfo_f32_s));
}
/****************************************************************************
* Name: motor_observer_nfo
*
* Description:
* nolinear fluxlink observer.
* REFERENCE: http://cas.ensmp.fr/~praly/Telechargement/Journaux/
* 2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
*
* Input Parameters:
* o - (in/out) pointer to the common observer data
* i_ab - (in) inverter alpha-beta current
* v_ab - (in) inverter alpha-beta voltage
* phy - (in) pointer to the motor physical parameters
* gain - (in) dynamic observer gain
*
* Returned Value:
* None
*
****************************************************************************/
void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
FAR struct motor_phy_params_f32_s *phy, float gain)
{
FAR struct motor_observer_nfo_f32_s *nfo =
(FAR struct motor_observer_nfo_f32_s *)o->ao;
float angle;
float err;
float x1_dot;
float x2_dot;
float l_ia = (3.0 / 2.0) * phy->ind * i_ab->a;
float l_ib = (3.0 / 2.0) * phy->ind * i_ab->b;
float r_ia = (3.0 / 2.0) * phy->res * i_ab->a;
float r_ib = (3.0 / 2.0) * phy->res * i_ab->b;
err = SQ(phy->flux_link) - (SQ(nfo->x1 - l_ia) + SQ(nfo->x2 - l_ib));
/* Forcing this term to stay negative helps convergence according to
* http://cas.ensmp.fr/Publications/Publications/Papers/
* ObserverPermanentMagnet.pdf and
* https://arxiv.org/pdf/1905.00833.pdf
*/
if (err > 0.0)
{
err = 0.0;
}
x1_dot = -r_ia + v_ab->a + gain * (nfo->x1 - l_ia) * err;
x2_dot = -r_ib + v_ab->b + gain * (nfo->x2 - l_ib) * err;
nfo->x1 += x1_dot * o->per;
nfo->x2 += x2_dot * o->per;
NAN_ZERO(nfo->x1);
NAN_ZERO(nfo->x2);
/* Prevent the magnitude from getting too low
* as that makes the angle very unstable.
*/
if (vector2d_mag(nfo->x1, nfo->x2) < (phy->flux_link * 0.5))
{
nfo->x1 *= 1.1;
nfo->x2 *= 1.1;
}
angle = fast_atan2(nfo->x2 - l_ib, nfo->x1 - l_ia);
/* Normalize angle to range <0, 2PI> */
angle_norm_2pi(&angle, 0.0f, 2.0f * M_PI_F);
/* Store estimated angle in observer data */
o->angle = angle;
}
/****************************************************************************
* Name: motor_sobserver_pll_init
*
* Description:
* Initialize PLL speed observer
*
* Input Parameters:
* so - (in/out) pointer to the PLL speed observer data
* pll_kp - (in) pll proportional gain
* pll_ki - (in) pll integral gain
*
* Returned Value:
* None
*
****************************************************************************/
void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
float pll_kp, float pll_ki)
{
LIBDSP_DEBUGASSERT(so != NULL);
LIBDSP_DEBUGASSERT(pll_kp > 0);
LIBDSP_DEBUGASSERT(pll_ki > 0.0f);
/* Reset observer data */
memset(so, 0, sizeof(struct motor_sobserver_pll_f32_s));
/* Store kp for PLL observer */
so->pll_kp = pll_kp;
/* Store ki for PLL observer speed */
so->pll_ki = pll_ki;
}
/****************************************************************************
* Name: motor_sobserver_pll
*
* Description:
* Estimate motor electrical speed based on motor electrical angle
* difference.
*
* Input Parameters:
* o - (in/out) pointer to the common observer data
* angle - (in) electrical angle normalized to <0.0, 2PI>
* dir - (in) electrical rotation direction. Valid values:
* DIR_CW (1.0f) or DIR_CCW(-1.0f)
*
****************************************************************************/
void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
float angle, float dir)
{
FAR struct motor_sobserver_pll_f32_s *so =
(FAR struct motor_sobserver_pll_f32_s *)o->so;
float delta_theta = 0.0;
NAN_ZERO(so->pll_phase);
/* Normalize angle to range <-PI, PI> */
angle_norm_2pi(&angle, -M_PI_F, -M_PI_F);
delta_theta = angle - so->pll_phase;
/* Normalize angle to range <-PI, PI> */
angle_norm_2pi(&delta_theta, -M_PI_F, -M_PI_F);
NAN_ZERO(o->speed);
so->pll_phase += (o->speed + so->pll_kp * delta_theta) * o->per;
/* Normalize angle to range <-PI, PI> */
angle_norm_2pi(&so->pll_phase, -M_PI_F, -M_PI_F);
o->speed += so->pll_ki * delta_theta * o->per;
}
/****************************************************************************
* Name: motor_observer_speed_get
*