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

libdsp: add _f32_ suffix to all libdsp data structures tht use float as a numerical type.

Browse Source 
This is an initial step to support fixed16 calculations for libdsp
This commit is contained in:
raiden00pl 2021-02-28 21:36:51 +01:00 committed by Xiang Xiao
parent f99f590751
commit 66972d947e
8 changed files with 187 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

198
include/dsp.h
View File

@ -131,14 +131,14 @@
* Besides angle value it also stores sine and cosine values for given angle.
*/
struct phase_angle_s
struct phase_angle_f32_s
{
float angle; /* Phase angle in radians <0, 2PI> */
float sin; /* Phase angle sine */
float cos; /* Phase angle cosine */
};
typedef struct phase_angle_s phase_angle_t;
typedef struct phase_angle_f32_s phase_angle_f32_t;
/* This structure stores motor angles and corresponding sin and cos values
*
@ -153,78 +153,78 @@ typedef struct phase_angle_s phase_angle_t;
* NOTE: pole_pairs = poles_total/2
*/
struct motor_angle_s
struct motor_angle_f32_s
{
phase_angle_t angle_el; /* Electrical angle */
float anglem; /* Mechanical angle in radians <0, 2PI> */
float one_by_p; /* Aux variable */
uint8_t p; /* Number of the motor pole pairs */
int8_t i; /* Pole counter */
phase_angle_f32_t angle_el; /* Electrical angle */
float anglem; /* Mechanical angle in radians <0, 2PI> */
float one_by_p; /* Aux variable */
uint8_t p; /* Number of the motor pole pairs */
int8_t i; /* Pole counter */
};
/* Float number saturaton */
struct float_sat_s
struct float_sat_f32_s
{
float min; /* Lower limit */
float max; /* Upper limit */
};
typedef struct float_sat_s float_sat_t;
typedef struct float_sat_f32_s float_sat_f32_t;
/* PI/PID controller state structure */
struct pid_controller_s
struct pid_controller_f32_s
{
float out; /* Controller output */
float_sat_t sat; /* Output saturation */
float err; /* Current error value */
float err_prev; /* Previous error value */
float KP; /* Proportional coefficient */
float KI; /* Integral coefficient */
float KD; /* Derivative coefficient */
float part[3]; /* 0 - proporitonal part
float out; /* Controller output */
float_sat_f32_t sat; /* Output saturation */
float err; /* Current error value */
float err_prev; /* Previous error value */
float KP; /* Proportional coefficient */
float KI; /* Integral coefficient */
float KD; /* Derivative coefficient */
float part[3]; /* 0 - proporitonal part
* 1 - integral part
* 2 - derivative part
*/
};
typedef struct pid_controller_s pid_controller_t;
typedef struct pid_controller_f32_s pid_controller_f32_t;
/* This structure represents the ABC frame (3 phase vector) */
struct abc_frame_s
struct abc_frame_f32_s
{
float a; /* A component */
float b; /* B component */
float c; /* C component */
};
typedef struct abc_frame_s abc_frame_t;
typedef struct abc_frame_f32_s abc_frame_f32_t;
/* This structure represents the alpha-beta frame (2 phase vector) */
struct ab_frame_s
struct ab_frame_f32_s
{
float a; /* Alpha component */
float b; /* Beta component */
};
typedef struct ab_frame_s ab_frame_t;
typedef struct ab_frame_f32_s ab_frame_f32_t;
/* This structure represent the direct-quadrature frame */
struct dq_frame_s
struct dq_frame_f32_s
{
float d; /* Driect component */
float q; /* Quadrature component */
};
typedef struct dq_frame_s dq_frame_t;
typedef struct dq_frame_f32_s dq_frame_f32_t;
/* Space Vector Modulation data for 3-phase system */
struct svm3_state_s
struct svm3_state_f32_s
{
uint8_t sector; /* Current space vector sector */
float d_u; /* Duty cycle for phase U */
@ -236,7 +236,7 @@ struct svm3_state_s
/* Motor open-loop control data */
struct openloop_data_s
struct openloop_data_f32_s
{
float max; /* Open-loop max speed */
float angle; /* Open-loop current angle normalized to <0.0, 2PI> */
@ -245,7 +245,7 @@ struct openloop_data_s
/* Common motor observer structure */
struct motor_observer_s
struct motor_observer_f32_s
{
float angle; /* Estimated observer angle */
float speed; /* Estimated observer speed */
@ -267,7 +267,7 @@ struct motor_observer_s
/* Speed observer division method data */
struct motor_sobserver_div_s
struct motor_sobserver_div_f32_s
{
float angle_diff; /* Mechanical angle difference */
float angle_acc; /* Accumulated mechanical angle */
@ -280,7 +280,7 @@ struct motor_sobserver_div_s
/* Speed observer PLL method data */
#if 0
struct motor_sobserver_pll_s
struct motor_sobserver_pll_f32_s
{
/* TODO */
};
@ -288,7 +288,7 @@ struct motor_sobserver_pll_s
/* Motor Sliding Mode Observer private data */
struct motor_observer_smo_s
struct motor_observer_smo_f32_s
{
float k_slide; /* Bang-bang controller gain */
float err_max; /* Linear mode threshold */
@ -296,13 +296,13 @@ struct motor_observer_smo_s
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 */
ab_frame_t emf_f; /* Fitlered estimated back-EMF */
ab_frame_t z; /* Correction factor */
ab_frame_t i_est; /* Estimated idq current */
ab_frame_t v_err; /* v_err = v_ab - emf */
ab_frame_t i_err; /* i_err = i_est - i_dq */
ab_frame_t sign; /* Bang-bang controller sign */
ab_frame_f32_t emf; /* Estimated back-EMF */
ab_frame_f32_t emf_f; /* Fitlered estimated back-EMF */
ab_frame_f32_t z; /* Correction factor */
ab_frame_f32_t i_est; /* Estimated idq current */
ab_frame_f32_t v_err; /* v_err = v_ab - emf */
ab_frame_f32_t i_err; /* i_err = i_est - i_dq */
ab_frame_f32_t sign; /* Bang-bang controller sign */
};
/* Motor physical parameters.
@ -310,7 +310,7 @@ struct motor_observer_smo_s
* but probably can be used to describe different types of motors.
*/
struct motor_phy_params_s
struct motor_phy_params_f32_s
{
uint8_t p; /* Number of the motor pole pairs */
@ -328,23 +328,24 @@ struct motor_phy_params_s
* REVISIT:
*/
struct foc_data_s
struct foc_data_f32_s
{
abc_frame_t v_abc; /* Voltage in ABC frame */
ab_frame_t v_ab; /* Voltage in alpha-beta frame */
dq_frame_t v_dq; /* Voltage in dq frame */
ab_frame_t v_ab_mod; /* Modulation voltage normalized to
abc_frame_f32_t v_abc; /* Voltage in ABC frame */
ab_frame_f32_t v_ab; /* Voltage in alpha-beta frame */
dq_frame_f32_t v_dq; /* Voltage in dq frame */
ab_frame_f32_t v_ab_mod; /* Modulation voltage normalized to
* magnitude (0.0, 1.0)
*/
abc_frame_t i_abc; /* Current in ABC 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 */
abc_frame_f32_t i_abc; /* Current in ABC frame */
ab_frame_f32_t i_ab; /* Current in apha-beta frame */
dq_frame_f32_t i_dq; /* Current in dq frame */
dq_frame_f32_t i_dq_err; /* DQ current error */
dq_frame_t i_dq_ref; /* Current dq reference frame */
pid_controller_t id_pid; /* Current d-axis component PI controller */
pid_controller_t iq_pid; /* Current q-axis component PI controller */
dq_frame_f32_t i_dq_ref; /* Current dq reference frame */
pid_controller_f32_t id_pid; /* Current d-axis component PI controller */
pid_controller_f32_t iq_pid; /* Current q-axis component PI controller */
float vdq_mag_max; /* Maximum dq voltage magnitude */
float vab_mod_scale; /* Voltage alpha-beta modulation scale */
@ -376,95 +377,96 @@ void f_saturate(FAR float *val, float min, float max);
float vector2d_mag(float x, float y);
void vector2d_saturate(FAR float *x, FAR float *y, float max);
void dq_saturate(FAR dq_frame_t *dq, float max);
float dq_mag(FAR dq_frame_t *dq);
void dq_saturate(FAR dq_frame_f32_t *dq, float max);
float dq_mag(FAR dq_frame_f32_t *dq);
/* PID controller functions */
void pid_controller_init(FAR pid_controller_t *pid,
void pid_controller_init(FAR pid_controller_f32_t *pid,
float KP, float KI, float KD);
void pi_controller_init(FAR pid_controller_t *pid,
void pi_controller_init(FAR pid_controller_f32_t *pid,
float KP, float KI);
void pid_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pi_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pid_integral_reset(FAR pid_controller_t *pid);
void pi_integral_reset(FAR pid_controller_t *pid);
float pi_controller(FAR pid_controller_t *pid, float err);
float pid_controller(FAR pid_controller_t *pid, float err);
void pid_saturation_set(FAR pid_controller_f32_t *pid, float min, float max);
void pi_saturation_set(FAR pid_controller_f32_t *pid, float min, float max);
void pid_integral_reset(FAR pid_controller_f32_t *pid);
void pi_integral_reset(FAR pid_controller_f32_t *pid);
float pi_controller(FAR pid_controller_f32_t *pid, float err);
float pid_controller(FAR pid_controller_f32_t *pid, float err);
/* Transformation functions */
void clarke_transform(FAR abc_frame_t *abc, FAR ab_frame_t *ab);
void inv_clarke_transform(FAR ab_frame_t *ab, FAR abc_frame_t *abc);
void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab,
FAR dq_frame_t *dq);
void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq,
FAR ab_frame_t *ab);
void clarke_transform(FAR abc_frame_f32_t *abc, FAR ab_frame_f32_t *ab);
void inv_clarke_transform(FAR ab_frame_f32_t *ab, FAR abc_frame_f32_t *abc);
void park_transform(FAR phase_angle_f32_t *angle, FAR ab_frame_f32_t *ab,
FAR dq_frame_f32_t *dq);
void inv_park_transform(FAR phase_angle_f32_t *angle, FAR dq_frame_f32_t *dq,
FAR ab_frame_f32_t *ab);
/* Phase angle related functions */
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);
void phase_angle_update(FAR struct phase_angle_f32_s *angle, float val);
/* 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);
void svm3_current_correct(FAR struct svm3_state_s *s,
void svm3_init(FAR struct svm3_state_f32_s *s, float min, float max);
void svm3(FAR struct svm3_state_f32_s *s, FAR ab_frame_f32_t *ab);
void svm3_current_correct(FAR struct svm3_state_f32_s *s,
int32_t *c0, int32_t *c1, int32_t *c2);
/* Field Oriented control */
void foc_vbase_update(FAR struct foc_data_s *foc, float vbase);
void foc_idq_ref_set(FAR struct foc_data_s *data, float d, float q);
void foc_vbase_update(FAR struct foc_data_f32_s *foc, float vbase);
void foc_idq_ref_set(FAR struct foc_data_f32_s *data, float d, float q);
void foc_init(FAR struct foc_data_s *data,
void foc_init(FAR struct foc_data_f32_s *data,
float id_kp, float id_ki, float iq_kp, float iq_ki);
void foc_process(FAR struct foc_data_s *foc,
FAR abc_frame_t *i_abc,
FAR phase_angle_t *angle);
void foc_process(FAR struct foc_data_f32_s *foc,
FAR abc_frame_f32_t *i_abc,
FAR phase_angle_f32_t *angle);
/* BLDC/PMSM motor observers */
void motor_observer_init(FAR struct motor_observer_s *observer,
void motor_observer_init(FAR struct motor_observer_f32_s *observer,
FAR void *ao, FAR void *so, float per);
float motor_observer_speed_get(FAR struct motor_observer_s *o);
float motor_observer_angle_get(FAR struct motor_observer_s *o);
float motor_observer_speed_get(FAR struct motor_observer_f32_s *o);
float motor_observer_angle_get(FAR struct motor_observer_f32_s *o);
void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
float kslide, float err_max);
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);
void motor_observer_smo(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 dir);
void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
uint8_t samples, float filer, float per);
void motor_sobserver_div(FAR struct motor_observer_s *o,
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
float angle, float dir);
/* Motor openloop control */
void motor_openloop_init(FAR struct openloop_data_s *op,
void motor_openloop_init(FAR struct openloop_data_f32_s *op,
float max, float per);
void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir);
float motor_openloop_angle_get(FAR struct openloop_data_s *op);
void motor_openloop(FAR struct openloop_data_f32_s *op, float speed,
float dir);
float motor_openloop_angle_get(FAR struct openloop_data_f32_s *op);
/* Motor angle */
void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p);
void motor_angle_e_update(FAR struct motor_angle_s *angle,
void motor_angle_init(FAR struct motor_angle_f32_s *angle, uint8_t p);
void motor_angle_e_update(FAR struct motor_angle_f32_s *angle,
float angle_new, float dir);
void motor_angle_m_update(FAR struct motor_angle_s *angle,
void motor_angle_m_update(FAR struct motor_angle_f32_s *angle,
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);
float motor_angle_m_get(FAR struct motor_angle_f32_s *angle);
float motor_angle_e_get(FAR struct motor_angle_f32_s *angle);
/* Motor physical parameters functions */
void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
float res, float ind);
void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
void motor_phy_params_init(FAR struct motor_phy_params_f32_s *phy,
uint8_t poles, float res, float ind);
void motor_phy_params_temp_set(FAR struct motor_phy_params_f32_s *phy,
float res_alpha, float res_temp_ref);
#undef EXTERN

25
libs/libdsp/lib_foc.c
View File

@ -45,11 +45,11 @@
*
****************************************************************************/
static void foc_current_control(FAR struct foc_data_s *foc)
static void foc_current_control(FAR struct foc_data_f32_s *foc)
{
FAR pid_controller_t *id_pid = &foc->id_pid;
FAR pid_controller_t *iq_pid = &foc->iq_pid;
FAR dq_frame_t *v_dq = &foc->v_dq;
FAR pid_controller_f32_t *id_pid = &foc->id_pid;
FAR pid_controller_f32_t *iq_pid = &foc->iq_pid;
FAR dq_frame_f32_t *v_dq = &foc->v_dq;
/* Get dq current error */
@ -88,7 +88,8 @@ static void foc_current_control(FAR struct foc_data_s *foc)
*
****************************************************************************/
static void foc_vab_mod_scale_set(FAR struct foc_data_s *foc, float scale)
static void foc_vab_mod_scale_set(FAR struct foc_data_f32_s *foc,
float scale)
{
foc->vab_mod_scale = scale;
}
@ -108,7 +109,7 @@ static void foc_vab_mod_scale_set(FAR struct foc_data_s *foc, float scale)
*
****************************************************************************/
static void foc_vdq_mag_max_set(FAR struct foc_data_s *foc, float max)
static void foc_vdq_mag_max_set(FAR struct foc_data_f32_s *foc, float max)
{
foc->vdq_mag_max = max;
@ -140,12 +141,12 @@ static void foc_vdq_mag_max_set(FAR struct foc_data_s *foc, float max)
*
****************************************************************************/
void foc_init(FAR struct foc_data_s *foc,
void foc_init(FAR struct foc_data_f32_s *foc,
float id_kp, float id_ki, float iq_kp, float iq_ki)
{
/* Reset data */
memset(foc, 0, sizeof(struct foc_data_s));
memset(foc, 0, sizeof(struct foc_data_f32_s));
/* Initialize PI current d component */
@ -172,7 +173,7 @@ void foc_init(FAR struct foc_data_s *foc,
*
****************************************************************************/
void foc_idq_ref_set(FAR struct foc_data_s *foc, float d, float q)
void foc_idq_ref_set(FAR struct foc_data_f32_s *foc, float d, float q)
{
foc->i_dq_ref.d = d;
foc->i_dq_ref.q = q;
@ -228,9 +229,9 @@ void foc_vbase_update(FAR struct foc_data_s *foc, float vbase)
*
****************************************************************************/
void foc_process(FAR struct foc_data_s *foc,
FAR abc_frame_t *i_abc,
FAR phase_angle_t *angle)
void foc_process(FAR struct foc_data_f32_s *foc,
FAR abc_frame_f32_t *i_abc,
FAR phase_angle_f32_t *angle)
{
DEBUGASSERT(foc != NULL);
DEBUGASSERT(i_abc != NULL);

6
libs/libdsp/lib_misc.c
View File

@ -138,7 +138,7 @@ void vector2d_saturate(FAR float *x, FAR float *y, float max)
*
****************************************************************************/
float dq_mag(FAR dq_frame_t *dq)
float dq_mag(FAR dq_frame_f32_t *dq)
{
return vector2d_mag(dq->d, dq->q);
}
@ -158,7 +158,7 @@ float dq_mag(FAR dq_frame_t *dq)
*
****************************************************************************/
void dq_saturate(FAR dq_frame_t *dq, float max)
void dq_saturate(FAR dq_frame_f32_t *dq, float max)
{
vector2d_saturate(&dq->d, &dq->q, max);
}
@ -433,7 +433,7 @@ void angle_norm_2pi(FAR float *angle, float bottom, float top)
*
****************************************************************************/
void phase_angle_update(FAR struct phase_angle_s *angle, float val)
void phase_angle_update(FAR struct phase_angle_f32_s *angle, float val)
{
DEBUGASSERT(angle != NULL);

31
libs/libdsp/lib_motor.c
View File

@ -50,7 +50,7 @@
*
****************************************************************************/
void motor_openloop_init(FAR struct openloop_data_s *op, float max,
void motor_openloop_init(FAR struct openloop_data_f32_s *op, float max,
float per)
{
DEBUGASSERT(op != NULL);
@ -59,7 +59,7 @@ void motor_openloop_init(FAR struct openloop_data_s *op, float max,
/* Reset openloop structure */
memset(op, 0, sizeof(struct openloop_data_s));
memset(op, 0, sizeof(struct openloop_data_f32_s));
/* Initialize data */
@ -83,7 +83,8 @@ void motor_openloop_init(FAR struct openloop_data_s *op, float max,
*
****************************************************************************/
void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir)
void motor_openloop(FAR struct openloop_data_f32_s *op, float speed,
float dir)
{
DEBUGASSERT(op != NULL);
DEBUGASSERT(speed >= 0.0f);
@ -127,7 +128,7 @@ void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir)
*
****************************************************************************/
float motor_openloop_angle_get(FAR struct openloop_data_s *op)
float motor_openloop_angle_get(FAR struct openloop_data_f32_s *op)
{
DEBUGASSERT(op != NULL);
@ -179,14 +180,14 @@ 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_init(FAR struct motor_angle_f32_s *angle, uint8_t p)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(p > 0);
/* Reset structure */
memset(angle, 0, sizeof(struct motor_angle_s));
memset(angle, 0, sizeof(struct motor_angle_f32_s));
/* Store pole pairs */
@ -213,8 +214,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_f32_s *angle,
float angle_new, float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
@ -282,8 +283,8 @@ void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new,
*
****************************************************************************/
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_f32_s *angle,
float angle_new, float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
@ -322,7 +323,7 @@ void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new,
*
****************************************************************************/
float motor_angle_m_get(FAR struct motor_angle_s *angle)
float motor_angle_m_get(FAR struct motor_angle_f32_s *angle)
{
DEBUGASSERT(angle != NULL);
@ -343,7 +344,7 @@ float motor_angle_m_get(FAR struct motor_angle_s *angle)
*
****************************************************************************/
float motor_angle_e_get(FAR struct motor_angle_s *angle)
float motor_angle_e_get(FAR struct motor_angle_f32_s *angle)
{
DEBUGASSERT(angle != NULL);
@ -368,8 +369,8 @@ float motor_angle_e_get(FAR struct motor_angle_s *angle)
*
****************************************************************************/
void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
float res, float ind)
void motor_phy_params_init(FAR struct motor_phy_params_f32_s *phy,
uint8_t poles, float res, float ind)
{
DEBUGASSERT(phy != NULL);
@ -400,7 +401,7 @@ void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
*
****************************************************************************/
void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
void motor_phy_params_temp_set(FAR struct motor_phy_params_f32_s *phy,
float res_alpha, float res_temp_ref)
{
DEBUGASSERT(phy != NULL);

55
libs/libdsp/lib_observer.c
View File

@ -51,7 +51,7 @@
*
****************************************************************************/
void motor_observer_init(FAR struct motor_observer_s *observer,
void motor_observer_init(FAR struct motor_observer_f32_s *observer,
FAR void *ao, FAR void *so, float per)
{
DEBUGASSERT(observer != NULL);
@ -61,7 +61,7 @@ void motor_observer_init(FAR struct motor_observer_s *observer,
/* Reset observer data */
memset(observer, 0, sizeof(struct motor_observer_s));
memset(observer, 0, sizeof(struct motor_observer_f32_s));
/* Set observer period */
@ -92,9 +92,8 @@ void motor_observer_init(FAR struct motor_observer_s *observer,
*
****************************************************************************/
void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
float kslide,
float err_max)
void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
float kslide, float err_max)
{
DEBUGASSERT(smo != NULL);
DEBUGASSERT(kslide > 0.0f);
@ -102,7 +101,7 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
/* Reset structure */
memset(smo, 0, sizeof(struct motor_observer_smo_s));
memset(smo, 0, sizeof(struct motor_observer_smo_f32_s));
/* Initialize structure */
@ -167,24 +166,24 @@ 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)
void motor_observer_smo(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 dir)
{
DEBUGASSERT(o != NULL);
DEBUGASSERT(i_ab != NULL);
DEBUGASSERT(v_ab != NULL);
DEBUGASSERT(phy != NULL);
FAR struct motor_observer_smo_s *smo =
(FAR struct motor_observer_smo_s *)o->ao;
FAR ab_frame_t *emf = &smo->emf;
FAR ab_frame_t *emf_f = &smo->emf_f;
FAR ab_frame_t *z = &smo->z;
FAR ab_frame_t *i_est = &smo->i_est;
FAR ab_frame_t *v_err = &smo->v_err;
FAR ab_frame_t *i_err = &smo->i_err;
FAR ab_frame_t *sign = &smo->sign;
FAR struct motor_observer_smo_f32_s *smo =
(FAR struct motor_observer_smo_f32_s *)o->ao;
FAR ab_frame_f32_t *emf = &smo->emf;
FAR ab_frame_f32_t *emf_f = &smo->emf_f;
FAR ab_frame_f32_t *z = &smo->z;
FAR ab_frame_f32_t *i_est = &smo->i_est;
FAR ab_frame_f32_t *v_err = &smo->v_err;
FAR ab_frame_f32_t *i_err = &smo->i_err;
FAR ab_frame_f32_t *sign = &smo->sign;
float i_err_a_abs = 0.0f;
float i_err_b_abs = 0.0f;
float angle = 0.0f;
@ -380,10 +379,8 @@ void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
*
****************************************************************************/
void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
uint8_t samples,
float filter,
float per)
void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
uint8_t samples, float filter, float per)
{
DEBUGASSERT(so != NULL);
DEBUGASSERT(samples > 0);
@ -391,7 +388,7 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
/* Reset observer data */
memset(so, 0, sizeof(struct motor_sobserver_div_s));
memset(so, 0, sizeof(struct motor_sobserver_div_f32_s));
/* Store number of samples for DIV observer */
@ -421,15 +418,15 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
*
****************************************************************************/
void motor_sobserver_div(FAR struct motor_observer_s *o,
float angle, float dir)
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
float angle, float dir)
{
DEBUGASSERT(o != NULL);
DEBUGASSERT(angle >= 0.0f && angle <= 2*M_PI_F);
DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
FAR struct motor_sobserver_div_s *so =
(FAR struct motor_sobserver_div_s *)o->so;
FAR struct motor_sobserver_div_f32_s *so =
(FAR struct motor_sobserver_div_f32_s *)o->so;
volatile float omega = 0.0f;
/* Get angle diff */
@ -516,7 +513,7 @@ void motor_sobserver_div(FAR struct motor_observer_s *o,
*
****************************************************************************/
float motor_observer_speed_get(FAR struct motor_observer_s *o)
float motor_observer_speed_get(FAR struct motor_observer_f32_s *o)
{
DEBUGASSERT(o != NULL);
@ -537,7 +534,7 @@ float motor_observer_speed_get(FAR struct motor_observer_s *o)
*
****************************************************************************/
float motor_observer_angle_get(FAR struct motor_observer_s *o)
float motor_observer_angle_get(FAR struct motor_observer_f32_s *o)
{
DEBUGASSERT(o != NULL);

20
libs/libdsp/lib_pid.c
View File

@ -46,14 +46,14 @@
*
****************************************************************************/
void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI,
void pid_controller_init(FAR pid_controller_f32_t *pid, float KP, float KI,
float KD)
{
DEBUGASSERT(pid != NULL);
/* Reset controller data */
memset(pid, 0, sizeof(pid_controller_t));
memset(pid, 0, sizeof(pid_controller_f32_t));
/* Copy controller parameters */
@ -79,13 +79,13 @@ void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI,
*
****************************************************************************/
void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI)
void pi_controller_init(FAR pid_controller_f32_t *pid, float KP, float KI)
{
DEBUGASSERT(pid != NULL);
/* Reset controller data */
memset(pid, 0, sizeof(pid_controller_t));
memset(pid, 0, sizeof(pid_controller_f32_t));
/* Copy controller parameters */
@ -112,7 +112,7 @@ void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI)
*
****************************************************************************/
void pid_saturation_set(FAR pid_controller_t *pid, float min, float max)
void pid_saturation_set(FAR pid_controller_f32_t *pid, float min, float max)
{
DEBUGASSERT(pid != NULL);
DEBUGASSERT(min < max);
@ -136,7 +136,7 @@ void pid_saturation_set(FAR pid_controller_t *pid, float min, float max)
*
****************************************************************************/
void pi_saturation_set(FAR pid_controller_t *pid, float min, float max)
void pi_saturation_set(FAR pid_controller_f32_t *pid, float min, float max)
{
DEBUGASSERT(pid != NULL);
DEBUGASSERT(min < max);
@ -148,7 +148,7 @@ void pi_saturation_set(FAR pid_controller_t *pid, float min, float max)
* Name: pid_integral_reset
****************************************************************************/
void pid_integral_reset(FAR pid_controller_t *pid)
void pid_integral_reset(FAR pid_controller_f32_t *pid)
{
pid->part[1] = 0.0f;
}
@ -157,7 +157,7 @@ void pid_integral_reset(FAR pid_controller_t *pid)
* Name: pi_integral_reset
****************************************************************************/
void pi_integral_reset(FAR pid_controller_t *pid)
void pi_integral_reset(FAR pid_controller_f32_t *pid)
{
pid_integral_reset(pid);
}
@ -177,7 +177,7 @@ void pi_integral_reset(FAR pid_controller_t *pid)
*
****************************************************************************/
float pi_controller(FAR pid_controller_t *pid, float err)
float pi_controller(FAR pid_controller_f32_t *pid, float err)
{
DEBUGASSERT(pid != NULL);
@ -252,7 +252,7 @@ float pi_controller(FAR pid_controller_t *pid, float err)
*
****************************************************************************/
float pid_controller(FAR pid_controller_t *pid, float err)
float pid_controller(FAR pid_controller_f32_t *pid, float err)
{
DEBUGASSERT(pid != NULL);

17
libs/libdsp/lib_svm.c
View File

@ -44,7 +44,7 @@
*
****************************************************************************/
static uint8_t svm3_sector_get(FAR abc_frame_t *ijk)
static uint8_t svm3_sector_get(FAR abc_frame_f32_t *ijk)
{
uint8_t sector = 0;
float i = ijk->a;
@ -135,7 +135,8 @@ static uint8_t svm3_sector_get(FAR abc_frame_t *ijk)
*
****************************************************************************/
static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
static void svm3_duty_calc(FAR struct svm3_state_f32_s *s,
FAR abc_frame_f32_t *ijk)
{
float i = ijk->a;
float j = ijk->b;
@ -320,12 +321,12 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
*
****************************************************************************/
void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab)
void svm3(FAR struct svm3_state_f32_s *s, FAR ab_frame_f32_t *v_ab)
{
DEBUGASSERT(s != NULL);
DEBUGASSERT(v_ab != NULL);
abc_frame_t ijk;
abc_frame_f32_t ijk;
/* Perform modified inverse Clarke-transformation (alpha,beta) -> (i,j,k)
* to obtain auxiliary frame which will be used in further calculations.
@ -359,8 +360,8 @@ void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab)
*
****************************************************************************/
void svm3_current_correct(FAR struct svm3_state_s *s,
int32_t *c0, int32_t *c1, int32_t *c2)
void svm3_current_correct(FAR struct svm3_state_f32_s *s,
int32_t *c0, int32_t *c1, int32_t *c2)
{
/* Get best ADC samples according to SVM sector.
*
@ -432,12 +433,12 @@ void svm3_current_correct(FAR struct svm3_state_s *s,
*
****************************************************************************/
void svm3_init(FAR struct svm3_state_s *s, float min, float max)
void svm3_init(FAR struct svm3_state_f32_s *s, float min, float max)
{
DEBUGASSERT(s != NULL);
DEBUGASSERT(max > min);
memset(s, 0, sizeof(struct svm3_state_s));
memset(s, 0, sizeof(struct svm3_state_f32_s));
s->d_max = max;
s->d_min = min;

20
libs/libdsp/lib_transform.c
View File

@ -50,8 +50,8 @@
*
****************************************************************************/
void clarke_transform(FAR abc_frame_t *abc,
FAR ab_frame_t *ab)
void clarke_transform(FAR abc_frame_f32_t *abc,
FAR ab_frame_f32_t *ab)
{
DEBUGASSERT(abc != NULL);
DEBUGASSERT(ab != NULL);
@ -75,8 +75,8 @@ void clarke_transform(FAR abc_frame_t *abc,
*
****************************************************************************/
void inv_clarke_transform(FAR ab_frame_t *ab,
FAR abc_frame_t *abc)
void inv_clarke_transform(FAR ab_frame_f32_t *ab,
FAR abc_frame_f32_t *abc)
{
DEBUGASSERT(ab != NULL);
DEBUGASSERT(abc != NULL);
@ -104,9 +104,9 @@ void inv_clarke_transform(FAR ab_frame_t *ab,
*
****************************************************************************/
void park_transform(FAR phase_angle_t *angle,
FAR ab_frame_t *ab,
FAR dq_frame_t *dq)
void park_transform(FAR phase_angle_f32_t *angle,
FAR ab_frame_f32_t *ab,
FAR dq_frame_f32_t *dq)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(ab != NULL);
@ -132,9 +132,9 @@ void park_transform(FAR phase_angle_t *angle,
*
****************************************************************************/
void inv_park_transform(FAR phase_angle_t *angle,
FAR dq_frame_t *dq,
FAR ab_frame_t *ab)
void inv_park_transform(FAR phase_angle_f32_t *angle,
FAR dq_frame_f32_t *dq,
FAR ab_frame_f32_t *ab)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(dq != NULL);