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

libs/libdsp: fix nxstyle issues

Browse Source
This commit is contained in:
raiden00pl 2020-04-03 21:18:18 +02:00 committed by Abdelatif Guettouche
parent 8876bd8ebc
commit abfb074110
8 changed files with 84 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
include/dsp.h
View File

@ -81,14 +81,14 @@
#define DIR_CW (1.0f)
#define DIR_CCW (-1.0f)
/* Some math constants *********************************************************/
/* Some math constants ******************************************************/
#define SQRT3_BY_TWO_F (0.866025f)
#define SQRT3_BY_THREE_F (0.57735f)
#define ONE_BY_SQRT3_F (0.57735f)
#define TWO_BY_SQRT3_F (1.15470f)
/* Some lib constants **********************************************************/
/* Some lib constants *******************************************************/
/* Motor electrical angle is in range 0.0 to 2*PI */
@ -102,7 +102,7 @@
#define MOTOR_ANGLE_M_MIN (0.0f)
#define MOTOR_ANGLE_M_RANGE (MOTOR_ANGLE_M_MAX - MOTOR_ANGLE_M_MIN)
/* Some useful macros ***************************************************************/
/* Some useful macros *******************************************************/
/****************************************************************************
* Name: LP_FILTER
@ -276,7 +276,8 @@ struct motor_observer_s
float angle_err; /* Observer angle error.
* This can be used to gradually eliminate
* error between openloop angle and observer angle
* error between openloop angle and observer
* angle
*/
/* There are different types of motor observers which different
@ -314,8 +315,8 @@ struct motor_observer_smo_s
{
float k_slide; /* Bang-bang controller gain */
float err_max; /* Linear mode threshold */
float F_gain; /* Current observer F gain (1-Ts*R/L) */
float G_gain; /* Current observer G gain (Ts/L) */
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 */
float emf_lp_filter2; /* Adaptive second low pass EMF filter */
ab_frame_t emf; /* Estimated back-EMF */
@ -360,7 +361,7 @@ struct foc_data_s
*/
abc_frame_t i_abc; /* Current in ABC frame */
ab_frame_t i_ab; /* Current in apha-beta frame*/
ab_frame_t i_ab; /* Current in apha-beta frame */
dq_frame_t i_dq; /* Current in dq frame */
dq_frame_t i_dq_err; /* DQ current error */
@ -373,7 +374,7 @@ struct foc_data_s
};
/****************************************************************************
* Public Functions
* Public Functions Prototypes
****************************************************************************/
#undef EXTERN
@ -429,7 +430,7 @@ void angle_norm(FAR float *angle, float per, float bottom, float top);
void angle_norm_2pi(FAR float *angle, float bottom, float top);
void phase_angle_update(FAR struct phase_angle_s *angle, float val);
/* 3-phase system space vector modulation*/
/* 3-phase system space vector modulation */
void svm3_init(FAR struct svm3_state_s *s, float min, float max);
void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *ab);
@ -476,9 +477,9 @@ float motor_openloop_angle_get(FAR struct openloop_data_s *op);
void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p);
void motor_angle_e_update(FAR struct motor_angle_s *angle,
float angle_new,float dir);
float angle_new, float dir);
void motor_angle_m_update(FAR struct motor_angle_s *angle,
float angle_new,float dir);
float angle_new, float dir);
float motor_angle_m_get(FAR struct motor_angle_s *angle);
float motor_angle_e_get(FAR struct motor_angle_s *angle);

6
libs/libdsp/lib_foc.c
View File

@ -42,10 +42,6 @@
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
@ -221,7 +217,7 @@ void foc_vbase_update(FAR struct foc_data_s *foc, float vbase)
if (vbase >= 0.0f)
{
scale = 1.0f/vbase;
scale = 1.0f / vbase;
mag_max = vbase;
}

13
libs/libdsp/lib_misc.c
View File

@ -43,9 +43,8 @@
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
#define VECTOR2D_SATURATE_MAG_MIN (1e-10f)
#define FAST_ATAN2_SMALLNUM (1e-10f)
/****************************************************************************
* Public Functions
@ -125,9 +124,9 @@ void vector2d_saturate(FAR float *x, FAR float *y, float max)
mag = vector2d_mag(*x, *y);
if (mag < 1e-10f)
if (mag < VECTOR2D_SATURATE_MAG_MIN)
{
mag = 1e-10f;
mag = VECTOR2D_SATURATE_MAG_MIN;
}
if (mag > max)
@ -327,7 +326,7 @@ float fast_cos2(float angle)
* Fast atan2 calculation
*
* REFERENCE:
* https://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization/
* https://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization/
*
* Input Parameters:
* x - (in)
@ -349,7 +348,7 @@ float fast_atan2(float y, float x)
/* Get absolute value of y and add some small number to prevent 0/0 */
abs_y = fabsf(y)+1e-10f;
abs_y = fabsf(y) + FAST_ATAN2_SMALLNUM;
/* Calculate angle */

27
libs/libdsp/lib_motor.c
View File

@ -43,11 +43,7 @@
* Pre-processor Definitions
****************************************************************************/
#define POLE_CNTR_THR 0.0f
/****************************************************************************
* Private Functions
****************************************************************************/
#define POLE_CNTR_THR (0.0f)
/****************************************************************************
* Public Functions
@ -69,7 +65,8 @@
*
****************************************************************************/
void motor_openloop_init(FAR struct openloop_data_s *op, float max, float per)
void motor_openloop_init(FAR struct openloop_data_s *op, float max,
float per)
{
DEBUGASSERT(op != NULL);
DEBUGASSERT(max > 0.0f);
@ -209,7 +206,7 @@ void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p)
/* Store pole pairs */
angle->p = p;
angle->one_by_p = (float)1.0f/p;
angle->one_by_p = (float)1.0f / p;
/* Initialize angle with 0.0 */
@ -231,7 +228,8 @@ void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p)
*
****************************************************************************/
void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new,
float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
@ -264,7 +262,7 @@ void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, floa
else if (angle->i < 0)
{
angle->i = angle->p-1;
angle->i = angle->p - 1;
}
/* Update electrical angle structure */
@ -272,8 +270,8 @@ void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, floa
phase_angle_update(&angle->angle_el, angle_new);
/* Calculate mechanical angle.
* One electrical angle rotation is equal to one mechanical rotation divided
* by number of motor pole pairs.
* One electrical angle rotation is equal to one mechanical rotation
* divided by number of motor pole pairs.
*/
angle->anglem = (MOTOR_ANGLE_E_RANGE * angle->i +
@ -299,7 +297,8 @@ void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, floa
*
****************************************************************************/
void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new,
float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
@ -313,7 +312,7 @@ void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new, floa
/* Update pole counter */
angle->i = (uint8_t)(angle->anglem * angle->p/MOTOR_ANGLE_M_MAX);
angle->i = (uint8_t)(angle->anglem * angle->p / MOTOR_ANGLE_M_MAX);
/* Get electrical angle */
@ -392,7 +391,7 @@ void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
phy->p = poles;
phy->res_base = res;
phy->ind = ind;
phy->one_by_ind = 1.0f/ind;
phy->one_by_ind = 1.0f / ind;
/* Initialize with zeros */

38
libs/libdsp/lib_observer.c
View File

@ -43,11 +43,7 @@
* Pre-processor Definitions
****************************************************************************/
#define ANGLE_DIFF_THR M_PI_F
/****************************************************************************
* Private Functions
****************************************************************************/
#define ANGLE_DIFF_THR (M_PI_F)
/****************************************************************************
* Public Functions
@ -187,8 +183,8 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
****************************************************************************/
void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy,
float dir)
FAR ab_frame_t *v_ab,
FAR struct motor_phy_params_s *phy, float dir)
{
DEBUGASSERT(o != NULL);
DEBUGASSERT(i_ab != NULL);
@ -213,21 +209,21 @@ void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
/* Calculate observer gains */
smo->F_gain = (1.0f - o->per*phy->res*phy->one_by_ind);
smo->G_gain = o->per*phy->one_by_ind;
smo->F = (1.0f - o->per * phy->res * phy->one_by_ind);
smo->G = o->per * phy->one_by_ind;
/* Saturate F gain */
if (smo->F_gain < 0.0f)
if (smo->F < 0.0f)
{
smo->F_gain = 0.0f;
smo->F = 0.0f;
}
/* Saturate G gain */
if (smo->G_gain > 0.999f)
if (smo->G > 0.999f)
{
smo->G_gain = 0.999f;
smo->G = 0.999f;
}
/* Configure low pass filters
@ -248,7 +244,8 @@ void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
* filter = T * (2*PI) * f_c
* filter = T * omega_m * pole_pairs
*
* T - [s] period at which the digital filter is being calculated
* T - [s] period at which the digital filter is being
* calculated
* f_in - [Hz] input frequency of the filter
* f_c - [Hz] cutoff frequency of the filter
* omega_m - [rad/s] mechanical angular velocity
@ -284,8 +281,8 @@ void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
/* Estimate stator current */
i_est->a = smo->F_gain * i_est->a + smo->G_gain * (v_err->a - z->a);
i_est->b = smo->F_gain * i_est->b + smo->G_gain * (v_err->b - z->b);
i_est->a = smo->F * i_est->a + smo->G * (v_err->a - z->a);
i_est->b = smo->F * i_est->b + smo->G * (v_err->b - z->b);
/* Get motor current error */
@ -365,8 +362,9 @@ void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
/* Angle compensation.
* Due to low pass filtering we have some delay in estimated phase angle.
*
* Adaptive filters introduced above cause -PI/4 phase shift for each filter.
* We use 2 times filtering which give us constant -PI/2 (-90deg) phase shift.
* Adaptive filters introduced above cause -PI/4 phase shift for each
* filter. We use 2 times filtering which give us constant -PI/2 (-90deg)
* phase shift.
*/
angle = angle + dir * M_PI_2_F;
@ -420,7 +418,7 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
/* */
so->one_by_dt = 1.0f/(so->samples * per);
so->one_by_dt = 1.0f / (so->samples * per);
}
/****************************************************************************
@ -462,7 +460,7 @@ void motor_sobserver_div(FAR struct motor_observer_s *o,
{
/* Correction sign depends on rotation direction */
so->angle_diff += dir*2*M_PI_F;
so->angle_diff += dir * 2 * M_PI_F;
}
/* Get absoulte value */

4
libs/libdsp/lib_pid.c
View File

@ -39,10 +39,6 @@
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/

59
libs/libdsp/lib_svm.c
View File

@ -41,10 +41,6 @@
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
@ -173,36 +169,42 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
T2 = j;
break;
}
case 2:
{
T1 = -k;
T2 = -i;
break;
}
case 3:
{
T1 = j;
T2 = k;
break;
}
case 4:
{
T1 = -i;
T2 = -j;
break;
}
case 5:
{
T1 = k;
T2 = i;
break;
}
case 6:
{
T1 = -j;
T2 = -k;
break;
}
default:
{
/* We should not get here */
@ -222,46 +224,52 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
{
case 1:
{
s->d_u = T1 + T2 + T0*0.5f;
s->d_v = T2 + T0*0.5f;
s->d_w = T0*0.5f;
s->d_u = T1 + T2 + T0 * 0.5f;
s->d_v = T2 + T0 * 0.5f;
s->d_w = T0 * 0.5f;
break;
}
case 2:
{
s->d_u = T1 + T0*0.5f;
s->d_v = T1 + T2 + T0*0.5f;
s->d_w = T0*0.5f;
s->d_u = T1 + T0 * 0.5f;
s->d_v = T1 + T2 + T0 * 0.5f;
s->d_w = T0 * 0.5f;
break;
}
case 3:
{
s->d_u = T0*0.5f;
s->d_v = T1 + T2 + T0*0.5f;
s->d_w = T2 + T0*0.5f;
s->d_u = T0 * 0.5f;
s->d_v = T1 + T2 + T0 * 0.5f;
s->d_w = T2 + T0 * 0.5f;
break;
}
case 4:
{
s->d_u = T0*0.5f;
s->d_v = T1 + T0*0.5f;
s->d_w = T1 + T2 + T0*0.5f;
s->d_u = T0 * 0.5f;
s->d_v = T1 + T0 * 0.5f;
s->d_w = T1 + T2 + T0 * 0.5f;
break;
}
case 5:
{
s->d_u = T2 + T0*0.5f;
s->d_v = T0*0.5f;
s->d_w = T1 + T2 + T0*0.5f;
s->d_u = T2 + T0 * 0.5f;
s->d_v = T0 * 0.5f;
s->d_w = T1 + T2 + T0 * 0.5f;
break;
}
case 6:
{
s->d_u = T1 + T2 + T0*0.5f;
s->d_v = T0*0.5f;
s->d_w = T1 + T0*0.5f;
s->d_u = T1 + T2 + T0 * 0.5f;
s->d_v = T0 * 0.5f;
s->d_w = T1 + T0 * 0.5f;
break;
}
default:
{
/* We should not get here */
@ -312,8 +320,8 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
*
* Input Parameters:
* s - (out) pointer to the SVM data
* v_ab - (in) pointer to the modulation voltage vector in alpha-beta frame,
* normalized to magnitude (0.0 - 1.0)
* v_ab - (in) pointer to the modulation voltage vector in alpha-beta
* frame, normalized to magnitude (0.0 - 1.0)
*
* NOTE: v_ab vector magnitude must be in range <0.0, 1.0> to get correct
* SVM3 results.
@ -322,7 +330,8 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
* For now we saturate output duty form SVM.
*
* REFERENCE:
* https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download (32-34)
* https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download
* pages 32-34
*
****************************************************************************/

4
libs/libdsp/lib_transform.c
View File

@ -39,10 +39,6 @@
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/