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

libdsp/lib_observer.c: optimize and add some comments

Browse Source
This commit is contained in:
raiden00pl 2021-03-01 19:56:36 +01:00 committed by Xiang Xiao
parent ee97eab4e7
commit eadc7ebb1a
2 changed files with 26 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/dsp.h
View File

@ -252,11 +252,11 @@ struct openloop_data_f32_s
struct motor_observer_f32_s
{
float angle; /* Estimated observer angle */
float speed; /* Estimated observer speed */
float per; /* Observer execution period */
float angle; /* Estimated observer angle */
float speed; /* Estimated observer speed */
float per; /* Observer execution period */
float angle_err; /* Observer angle error.
float angle_err; /* Observer angle error.
* This can be used to gradually eliminate
* error between openloop angle and observer
* angle
@ -274,13 +274,13 @@ struct motor_observer_f32_s
struct motor_sobserver_div_f32_s
{
float angle_diff; /* Mechanical angle difference */
float angle_acc; /* Accumulated mechanical angle */
float angle_prev; /* Previous mechanical angle */
float one_by_dt; /* Frequency of observer execution */
float cntr; /* Sample counter */
float samples; /* Number of samples for observer */
float filter; /* Low-pass filter for final omega */
float angle_diff; /* Mechanical angle difference */
float angle_acc; /* Accumulated mechanical angle */
float angle_prev; /* Previous mechanical angle */
float one_by_dt; /* Frequency of observer execution */
float cntr; /* Sample counter */
float samples; /* Number of samples for observer */
float filter; /* Low-pass filter for final omega */
};
/* Speed observer PLL method data */
@ -297,6 +297,7 @@ struct motor_observer_smo_f32_s
{
float k_slide; /* Bang-bang controller gain */
float err_max; /* Linear mode threshold */
float one_by_err_max; /* One by err_max */
float F; /* Current observer F gain (1-Ts*R/L) */
float G; /* Current observer G gain (Ts/L) */
float emf_lp_filter1; /* Adaptive first low pass EMF filter */

33
libs/libdsp/lib_observer.c
View File

@ -107,6 +107,10 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
smo->k_slide = kslide;
smo->err_max = err_max;
/* Store inverted err_max to avoid division */
smo->one_by_err_max = (1.0f/err_max);
}
/****************************************************************************
@ -159,7 +163,8 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
* i_ab - (in) inverter alpha-beta current
* v_ab - (in) inverter alpha-beta voltage
* phy - (in) pointer to the motor physical parameters
* dir - (in) rotation direction (1.0 for CW, -1.0 for CCW)
* dir - (in) rotation direction (1.0 for CCW, -1.0 for CW)
* NOTE: (mechanical dir) = -(electrical dir)
*
* Returned Value:
* None
@ -189,7 +194,9 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
float angle = 0.0f;
float filter = 0.0f;
/* REVISIT: observer works only when IQ current is high enough */
/* REVISIT: observer works only when IQ current is high enough
* Lower IQ current -> lower K_SLIDE
*/
/* Calculate observer gains */
@ -289,7 +296,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
{
/* Enter linear region if error is small enough */
z->a = i_err->a * smo->k_slide / smo->err_max;
z->a = i_err->a * smo->k_slide * smo->one_by_err_max;
}
else
{
@ -302,7 +309,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
{
/* Enter linear region if error is small enough */
z->b = i_err->b * smo->k_slide / smo->err_max;
z->b = i_err->b * smo->k_slide * smo->one_by_err_max;
}
else
{
@ -325,24 +332,12 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
* emf_a = -|emf| * sin(th)
* emf_b = |emf| * cos(th)
* th = atan2(-emf_a, emf->b)
*
* NOTE: bottleneck but we can't do much more to optimise this
*/
angle = fast_atan2(-emf->a, emf->b);
#if 1
/* Some assertions
* TODO: simplify
*/
if (angle != angle) angle = 0.0f;
if (emf->a != emf->a) emf->a = 0.0f;
if (emf->b != emf->b) emf->b = 0.0f;
if (z->a != z->a) z->a = 0.0f;
if (z->b != z->b) z->b = 0.0f;
if (i_est->a != i_est->a) i_est->a = 0.0f;
if (i_est->b != i_est->b) i_est->b = 0.0f;
#endif
/* Angle compensation.
* Due to low pass filtering we have some delay in estimated phase angle.
*
@ -398,7 +393,7 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
so->filter = filter;
/* */
/* Store inverted sampling period */
so->one_by_dt = 1.0f / (so->samples * per);
}