From 66972d947e3c6b4c1b2de13b75d7bf9a5898472e Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 28 Feb 2021 21:36:51 +0100 Subject: [PATCH] libdsp: add _f32_ suffix to all libdsp data structures tht use float as a numerical type. This is an initial step to support fixed16 calculations for libdsp --- include/dsp.h | 198 ++++++++++++++++++------------------ libs/libdsp/lib_foc.c | 25 ++--- libs/libdsp/lib_misc.c | 6 +- libs/libdsp/lib_motor.c | 31 +++--- libs/libdsp/lib_observer.c | 55 +++++----- libs/libdsp/lib_pid.c | 20 ++-- libs/libdsp/lib_svm.c | 17 ++-- libs/libdsp/lib_transform.c | 20 ++-- 8 files changed, 187 insertions(+), 185 deletions(-) diff --git a/include/dsp.h b/include/dsp.h index b7fbcc6a27..ac3d1ac908 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -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 diff --git a/libs/libdsp/lib_foc.c b/libs/libdsp/lib_foc.c index 65e2052330..3b0e02c3fe 100644 --- a/libs/libdsp/lib_foc.c +++ b/libs/libdsp/lib_foc.c @@ -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); diff --git a/libs/libdsp/lib_misc.c b/libs/libdsp/lib_misc.c index 7f5686c9e8..feb388be2e 100644 --- a/libs/libdsp/lib_misc.c +++ b/libs/libdsp/lib_misc.c @@ -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); diff --git a/libs/libdsp/lib_motor.c b/libs/libdsp/lib_motor.c index 6a3549a1df..fa0ccbe7ff 100644 --- a/libs/libdsp/lib_motor.c +++ b/libs/libdsp/lib_motor.c @@ -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); diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c index 6fab49f279..eb894668d1 100644 --- a/libs/libdsp/lib_observer.c +++ b/libs/libdsp/lib_observer.c @@ -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); diff --git a/libs/libdsp/lib_pid.c b/libs/libdsp/lib_pid.c index a6621809a5..534b8c1c6a 100644 --- a/libs/libdsp/lib_pid.c +++ b/libs/libdsp/lib_pid.c @@ -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); diff --git a/libs/libdsp/lib_svm.c b/libs/libdsp/lib_svm.c index 02b5b51cc3..c42582bd49 100644 --- a/libs/libdsp/lib_svm.c +++ b/libs/libdsp/lib_svm.c @@ -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; diff --git a/libs/libdsp/lib_transform.c b/libs/libdsp/lib_transform.c index b63e914091..50d4468e56 100644 --- a/libs/libdsp/lib_transform.c +++ b/libs/libdsp/lib_transform.c @@ -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);