From 255ffcf8521a16b91c6c227c47a7cf0df0f303cd Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Mon, 1 Mar 2021 20:39:42 +0100 Subject: [PATCH] libdsp: add support for fixed16 libdsp --- include/dsp.h | 5 +- include/dspb16.h | 480 +++++++++++++++++++++++++++++++ libs/libdsp/Makefile | 8 + libs/libdsp/lib_foc_b16.c | 464 ++++++++++++++++++++++++++++++ libs/libdsp/lib_misc_b16.c | 457 +++++++++++++++++++++++++++++ libs/libdsp/lib_motor_b16.c | 279 ++++++++++++++++++ libs/libdsp/lib_pid_b16.c | 352 +++++++++++++++++++++++ libs/libdsp/lib_pmsm_model_b16.c | 197 +++++++++++++ libs/libdsp/lib_svm_b16.c | 437 ++++++++++++++++++++++++++++ libs/libdsp/lib_transform_b16.c | 151 ++++++++++ 10 files changed, 2826 insertions(+), 4 deletions(-) create mode 100644 include/dspb16.h create mode 100644 libs/libdsp/lib_foc_b16.c create mode 100644 libs/libdsp/lib_misc_b16.c create mode 100644 libs/libdsp/lib_motor_b16.c create mode 100644 libs/libdsp/lib_pid_b16.c create mode 100644 libs/libdsp/lib_pmsm_model_b16.c create mode 100644 libs/libdsp/lib_svm_b16.c create mode 100644 libs/libdsp/lib_transform_b16.c diff --git a/include/dsp.h b/include/dsp.h index c8d9ef2660..723abe90b6 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -360,10 +360,7 @@ struct foc_data_f32_s struct motor_phy_params_f32_s { uint8_t p; /* Number of the motor pole pairs */ - - float res; /* Phase-to-neutral temperature compensated - * resistance - */ + float res; /* Phase-to-neutral resistance */ float ind; /* Average phase-to-neutral inductance */ float one_by_ind; /* Inverse phase-to-neutral inductance */ }; diff --git a/include/dspb16.h b/include/dspb16.h new file mode 100644 index 0000000000..47d0b8de16 --- /dev/null +++ b/include/dspb16.h @@ -0,0 +1,480 @@ +/**************************************************************************** + * include/dspb16.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __INCLUDE_DSPB16_H +#define __INCLUDE_DSPB16_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Disable DEBUGASSERT macro if LIBDSP debug is not enabled */ + +#ifdef CONFIG_LIBDSP_DEBUG +# ifndef CONFIG_DEBUG_ASSERTIONS +# warning "Need CONFIG_DEBUG_ASSERTIONS to work properly" +# endif +# define LIBDSP_DEBUGASSERT(x) DEBUGASSERT(x) +#else +# undef LIBDSP_DEBUGASSERT +# define LIBDSP_DEBUGASSERT(x) +#endif + +#ifndef CONFIG_LIBDSP_PRECISION +# define CONFIG_LIBDSP_PRECISION 0 +#endif + +/* Phase rotation direction */ + +#define DIR_NONE_B16 ftob16(0.0f) +#define DIR_CW_B16 ftob16(1.0f) +#define DIR_CCW_B16 ftob16(-1.0f) + +/* Some math constants ******************************************************/ + +#define SQRT3_BY_TWO_B16 ftob16(0.866025f) +#define SQRT3_BY_THREE_B16 ftob16(0.57735f) +#define ONE_BY_SQRT3_B16 ftob16(0.57735f) +#define TWO_BY_SQRT3_B16 ftob16(1.15470f) + +/* Some lib constants *******************************************************/ + +/* Motor electrical angle is in range 0.0 to 2*PI */ + +#define MOTOR_ANGLE_E_MAX_B16 (b16TWOPI) +#define MOTOR_ANGLE_E_MIN_B16 (0) +#define MOTOR_ANGLE_E_RANGE_B16 (MOTOR_ANGLE_E_MAX_B16 - MOTOR_ANGLE_E_MIN_B16) + +/* Motor mechanical angle is in range 0.0 to 2*PI */ + +#define MOTOR_ANGLE_M_MAX_B16 (b16TWOPI) +#define MOTOR_ANGLE_M_MIN_B16 (0) +#define MOTOR_ANGLE_M_RANGE_B16 (MOTOR_ANGLE_M_MAX_B16 - MOTOR_ANGLE_M_MIN_B16) + +/* Some useful macros *******************************************************/ + +/**************************************************************************** + * Name: LP_FILTER_B16 + * + * Description: + * Simple single-pole digital low pass filter: + * Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n))) + * + * filter - (0.0 - 1.0) where 1.0 gives unfiltered values + * filter = T * (2*PI) * f_c + * + * phase shift = -arctan(f_in/f_c) + * + * T - period at which the digital filter is being calculated + * f_in - input frequency of the filter + * f_c - cutoff frequency of the filter + * + * REFERENCE: https://www.embeddedrelated.com/showarticle/779.php + * + ****************************************************************************/ + +#define LP_FILTER_B16(val, sample, filter) val -= (b16mulb16(filter, (val - sample))) + +/**************************************************************************** + * Name: SVM3_BASE_VOLTAGE_GET_B16 + * + * Description: + * Get maximum voltage for SVM3 without overmodulation + * + * Notes: + * max possible phase voltage for 3-phase power inwerter: + * Vd = (2/3)*Vdc + * max phase reference voltage according to SVM modulation diagram: + * Vrefmax = Vd * cos(30*) = SQRT3_BY_2 * Vd + * which give us: + * Vrefmax = SQRT3_BY_3 * Vdc + * + * Vdc - bus voltage + * + ****************************************************************************/ + +#define SVM3_BASE_VOLTAGE_GET_B16(vbus) (b16mulb16(vbus, SQRT3_BY_THREE_B16)) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* This structure represents phase angle. + * Besides angle value it also stores sine and cosine values for given angle. + */ + +struct phase_angle_b16_s +{ + b16_t angle; /* Phase angle in radians <0, 2PI> */ + b16_t sin; /* Phase angle sine */ + b16_t cos; /* Phase angle cosine */ +}; + +typedef struct phase_angle_b16_s phase_angle_b16_t; + +/* This structure stores motor angles and corresponding sin and cos values + * + * th_el = th_m * pole_pairs + * th_m = th_el/pole_pairs + * + * where: + * th_el - motor electrical angle + * th_m - motor mechanical angle + * pole_pairs - motor pole pairs + * + * NOTE: pole_pairs = poles_total/2 + */ + +struct motor_angle_b16_s +{ + phase_angle_b16_t angle_el; /* Electrical angle */ + b16_t anglem; /* Mechanical angle in radians <0, 2PI> */ + b16_t 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_b16_s +{ + b16_t min; /* Lower limit */ + b16_t max; /* Upper limit */ +}; + +typedef struct float_sat_b16_s float_sat_b16_t; + +/* PI/PID controller state structure */ + +struct pid_controller_b16_s +{ + bool aw_en; /* Integral part decay if saturated */ + bool ireset_en; /* Intergral part reset if saturated */ + bool pisat_en; /* PI saturation enabled */ + bool pidsat_en; /* PID saturation enabled */ + bool _res; /* Reserved */ + b16_t out; /* Controller output */ + float_sat_b16_t sat; /* Output saturation */ + b16_t err; /* Current error value */ + b16_t err_prev; /* Previous error value */ + b16_t KP; /* Proportional coefficient */ + b16_t KI; /* Integral coefficient */ + b16_t KD; /* Derivative coefficient */ + b16_t part[3]; /* 0 - proporitonal part + * 1 - integral part + * 2 - derivative part + */ + b16_t KC; /* Integral anti-windup decay coefficient */ + b16_t aw; /* Integral anti-windup decay part */ +}; + +typedef struct pid_controller_b16_s pid_controller_b16_t; + +/* This structure represents the ABC frame (3 phase vector) */ + +struct abc_frame_b16_s +{ + b16_t a; /* A component */ + b16_t b; /* B component */ + b16_t c; /* C component */ +}; + +typedef struct abc_frame_b16_s abc_frame_b16_t; + +/* This structure represents the alpha-beta frame (2 phase vector) */ + +struct ab_frame_b16_s +{ + b16_t a; /* Alpha component */ + b16_t b; /* Beta component */ +}; + +typedef struct ab_frame_b16_s ab_frame_b16_t; + +/* This structure represent the direct-quadrature frame */ + +struct dq_frame_b16_s +{ + b16_t d; /* Driect component */ + b16_t q; /* Quadrature component */ +}; + +typedef struct dq_frame_b16_s dq_frame_b16_t; + +/* Space Vector Modulation data for 3-phase system */ + +struct svm3_state_b16_s +{ + uint8_t sector; /* Current space vector sector */ + b16_t d_u; /* Duty cycle for phase U */ + b16_t d_v; /* Duty cycle for phase V */ + b16_t d_w; /* Duty cycle for phase W */ +}; + +/* FOC initialize data */ + +struct foc_initdata_b16_s +{ + b16_t id_kp; /* KP for d current */ + b16_t id_ki; /* KI for d current */ + b16_t iq_kp; /* KP for q current */ + b16_t iq_ki; /* KI for q current */ +}; + +/* Field Oriented Control (FOC) data */ + +struct foc_data_b16_s +{ + abc_frame_b16_t v_abc; /* Voltage in ABC frame */ + ab_frame_b16_t v_ab; /* Voltage in alpha-beta frame */ + dq_frame_b16_t v_dq; /* Requested voltage in dq frame */ + ab_frame_b16_t v_ab_mod; /* Modulation voltage normalized to + * magnitude (0.0, 1.0) + */ + + abc_frame_b16_t i_abc; /* Current in ABC frame */ + ab_frame_b16_t i_ab; /* Current in alpha-beta frame */ + dq_frame_b16_t i_dq; /* Current in dq frame */ + dq_frame_b16_t i_dq_err; /* DQ current error */ + + dq_frame_b16_t i_dq_ref; /* Requested current for the FOC + * current controler + */ + + pid_controller_b16_t id_pid; /* Current d-axis component PI controller */ + pid_controller_b16_t iq_pid; /* Current q-axis component PI controller */ + + b16_t vdq_mag_max; /* Maximum dq voltage magnitude */ + b16_t vab_mod_scale; /* Voltage alpha-beta modulation scale */ + + phase_angle_b16_t angle; /* Phase angle */ +}; + +/* Motor physical parameters. + * This data structure was designed to work with BLDC/PMSM motors, + * but probably can be used to describe different types of motors. + */ + +struct motor_phy_params_b16_s +{ + uint8_t p; /* Number of the motor pole pairs */ + b16_t res; /* Phase-to-neutral temperature compensated + * resistance + */ + b16_t ind; /* Average phase-to-neutral inductance */ + b16_t one_by_ind; /* Inverse phase-to-neutral inductance */ +}; + +/* PMSM motor physcial parameters */ + +struct pmsm_phy_params_b16_s +{ + struct motor_phy_params_b16_s motor; /* Motor common PHY */ + b16_t iner; /* Rotor inertia */ + b16_t flux_link; /* Flux linkage */ + b16_t ind_d; /* d-inductance */ + b16_t ind_q; /* q-inductance */ + b16_t one_by_iner; /* One by J */ + b16_t one_by_indd; /* One by Ld */ + b16_t one_by_indq; /* One by Lq */ +}; + +/* PMSM motor model state */ + +struct pmsm_model_state_b16_s +{ + /* Motor model phase current */ + + abc_frame_b16_t i_abc; + ab_frame_b16_t i_ab; + dq_frame_b16_t i_dq; + + /* Motor model phase voltage */ + + abc_frame_b16_t v_abc; + ab_frame_b16_t v_ab; + dq_frame_b16_t v_dq; + + /* Motor model angle */ + + struct motor_angle_b16_s angle; + + /* Angular speed */ + + b16_t omega_e; + b16_t omega_m; +}; + +/* PMSM motor model external conditions */ + +struct pmsm_model_ext_b16_s +{ + b16_t load; /* Motor model load torque */ +}; + +/* PMSM motor model */ + +struct pmsm_model_b16_s +{ + struct pmsm_phy_params_b16_s phy; /* Motor model physical parameters */ + struct pmsm_model_state_b16_s state; /* Motor model state */ + struct pmsm_model_ext_b16_s ext; /* Motor model external conditions */ + b16_t per; /* Control period */ + b16_t id_int; /* Id integral part */ + b16_t iq_int; /* Iq integral part */ +}; + +/**************************************************************************** + * Public Functions Prototypes + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/* Math functions */ + +b16_t fast_sin_b16(b16_t angle); +b16_t fast_sin2_b16(b16_t angle); +b16_t fast_cos_b16(b16_t angle); +b16_t fast_cos2_b16(b16_t angle); +b16_t fast_atan2_b16(b16_t y, b16_t x); +void f_saturate_b16(FAR b16_t *val, b16_t min, b16_t max); +b16_t vector2d_mag_b16(b16_t x, b16_t y); +void vector2d_saturate_b16(FAR b16_t *x, FAR b16_t *y, b16_t max); +void dq_saturate_b16(FAR dq_frame_b16_t *dq, b16_t max); +b16_t dq_mag_b16(FAR dq_frame_b16_t *dq); + +/* PID controller functions */ + +void pid_controller_init_b16(FAR pid_controller_b16_t *pid, + b16_t KP, b16_t KI, b16_t KD); +void pi_controller_init_b16(FAR pid_controller_b16_t *pid, + b16_t KP, b16_t KI); +void pid_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min, + b16_t max); +void pi_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min, + b16_t max); +void pid_integral_reset_b16(FAR pid_controller_b16_t *pid); +void pi_integral_reset_b16(FAR pid_controller_b16_t *pid); +b16_t pi_controller_b16(FAR pid_controller_b16_t *pid, b16_t err); +b16_t pid_controller_b16(FAR pid_controller_b16_t *pid, b16_t err); +void pi_antiwindup_enable_b16(FAR pid_controller_b16_t *pid, b16_t KC, + bool enable); +void pi_ireset_enable_b16(FAR pid_controller_b16_t *pid, bool enable); + +/* Transformation functions */ + +void clarke_transform_b16(FAR abc_frame_b16_t *abc, FAR ab_frame_b16_t *ab); +void inv_clarke_transform_b16(FAR ab_frame_b16_t *ab, + FAR abc_frame_b16_t *abc); +void park_transform_b16(FAR phase_angle_b16_t *angle, FAR ab_frame_b16_t *ab, + FAR dq_frame_b16_t *dq); +void inv_park_transform_b16(FAR phase_angle_b16_t *angle, + FAR dq_frame_b16_t *dq, FAR ab_frame_b16_t *ab); + +/* Phase angle related functions */ + +void angle_norm_b16(FAR b16_t *angle, b16_t per, b16_t bottom, b16_t top); +void angle_norm_2pi_b16(FAR b16_t *angle, b16_t bottom, b16_t top); +void phase_angle_update_b16(FAR struct phase_angle_b16_s *angle, b16_t val); + +/* 3-phase system space vector modulation */ + +void svm3_init_b16(FAR struct svm3_state_b16_s *s); +void svm3_b16(FAR struct svm3_state_b16_s *s, FAR ab_frame_b16_t *ab); +void svm3_current_correct_b16(FAR struct svm3_state_b16_s *s, + b16_t *c0, b16_t *c1, b16_t *c2); + +/* Field Oriented Control */ + +void foc_init_b16(FAR struct foc_data_b16_s *foc, + FAR struct foc_initdata_b16_s *init); +void foc_vbase_update_b16(FAR struct foc_data_b16_s *foc, b16_t vbase); +void foc_angle_update_b16(FAR struct foc_data_b16_s *foc, + FAR phase_angle_b16_t *angle); +void foc_iabc_update_b16(FAR struct foc_data_b16_s *foc, + FAR abc_frame_b16_t *i_abc); +void foc_voltage_control_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *vdq_ref); +void foc_current_control_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *idq_ref, + FAR dq_frame_b16_t *vdq_comp, + FAR dq_frame_b16_t *v_dq_ref); +void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc, + FAR ab_frame_b16_t *v_ab_mod); +void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max); + +/* Motor angle */ + +void motor_angle_init_b16(FAR struct motor_angle_b16_s *angle, uint8_t p); +void motor_angle_e_update_b16(FAR struct motor_angle_b16_s *angle, + b16_t angle_new, b16_t dir); +void motor_angle_m_update_b16(FAR struct motor_angle_b16_s *angle, + b16_t angle_new, b16_t dir); +b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle); +b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle); + +/* Motor physical parameters */ + +void motor_phy_params_init_b16(FAR struct motor_phy_params_b16_s *phy, + uint8_t poles, b16_t res, b16_t ind); + +/* PMSM physical parameters functions */ + +void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy, + uint8_t poles, b16_t res, b16_t ind, + b16_t iner, b16_t flux, + b16_t ind_d, b16_t ind_q); + +/* PMSM motor model */ + +int pmsm_model_initialize_b16(FAR struct pmsm_model_b16_s *model, + FAR struct pmsm_phy_params_b16_s *phy, + b16_t per); +int pmsm_model_elec_b16(FAR struct pmsm_model_b16_s *model, + FAR ab_frame_b16_t *vab); +int pmsm_model_mech_b16(FAR struct pmsm_model_b16_s *model, b16_t load); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __INCLUDE_DSPB16_H */ diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile index 7b0ec47974..b308cfc50b 100644 --- a/libs/libdsp/Makefile +++ b/libs/libdsp/Makefile @@ -29,6 +29,14 @@ CSRCS += lib_foc.c CSRCS += lib_misc.c CSRCS += lib_motor.c CSRCS += lib_pmsm_model.c + +CSRCS += lib_pid_b16.c +CSRCS += lib_svm_b16.c +CSRCS += lib_transform_b16.c +CSRCS += lib_foc_b16.c +CSRCS += lib_misc_b16.c +CSRCS += lib_motor_b16.c +CSRCS += lib_pmsm_model_b16.c endif AOBJS = $(ASRCS:.S=$(OBJEXT)) diff --git a/libs/libdsp/lib_foc_b16.c b/libs/libdsp/lib_foc_b16.c new file mode 100644 index 0000000000..1a21a9f63f --- /dev/null +++ b/libs/libdsp/lib_foc_b16.c @@ -0,0 +1,464 @@ +/**************************************************************************** + * libs/libdsp/lib_foc_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_current_controler_b16 + * + * Description: + * This function implements FOC current controler algorithm. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * v_dq_req - (in) pointer to the voltage DQ request frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_current_controler_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *v_dq_req) +{ + FAR pid_controller_b16_t *id_pid = &foc->id_pid; + FAR pid_controller_b16_t *iq_pid = &foc->iq_pid; + + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(v_dq_req != NULL); + + /* Get dq current error */ + + foc->i_dq_err.d = foc->i_dq_ref.d - foc->i_dq.d; + foc->i_dq_err.q = foc->i_dq_ref.q - foc->i_dq.q; + + /* NOTE: PI controllers saturation is updated in foc_vdq_mag_max_set() */ + + /* PI controller for d-current (flux loop) */ + + v_dq_req->d = pi_controller_b16(id_pid, foc->i_dq_err.d); + + /* PI controller for q-current (torque loop) */ + + v_dq_req->q = pi_controller_b16(iq_pid, foc->i_dq_err.q); +} + +/**************************************************************************** + * Name: foc_vab_mod_scale_set_b16 + * + * Description: + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * scale - (in) scaling factor for alpha-beta voltage + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_vab_mod_scale_set_b16(FAR struct foc_data_b16_s *foc, + b16_t scale) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + foc->vab_mod_scale = scale; +} + +/**************************************************************************** + * Name: foc_vdq_mag_max_set_b16 + * + * Description: + * Set maximum dq voltage vector magnitude + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * max - (in) maximum dq voltage magnitude + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_vdq_mag_max_set_b16(FAR struct foc_data_b16_s *foc, + b16_t max) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + foc->vdq_mag_max = max; + + /* Update regulators saturation */ + + pi_saturation_set_b16(&foc->id_pid, -foc->vdq_mag_max, foc->vdq_mag_max); + pi_saturation_set_b16(&foc->iq_pid, -foc->vdq_mag_max, foc->vdq_mag_max); +} + +/**************************************************************************** + * Name: foc_vdq_ref_set_b16 + * + * Description: + * Set dq requested voltage vector + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * vdq_ref - (in) pointer to the requested idq voltage + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_vdq_ref_set_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(vdq_ref != NULL); + + foc->v_dq.d = vdq_ref->d; + foc->v_dq.q = vdq_ref->q; +} + +/**************************************************************************** + * Name: foc_idq_ref_set_b16 + * + * Description: + * Set dq reference current vector + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * idq_ref - (in) pointer to the reference idq current + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void foc_idq_ref_set_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *idq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(idq_ref != NULL); + + foc->i_dq_ref.d = idq_ref->d; + foc->i_dq_ref.q = idq_ref->q; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_init_b16 + * + * Description: + * Initialize FOC controller + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * init - (in) pointer to the FOC initialization data + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_init_b16(FAR struct foc_data_b16_s *foc, + FAR struct foc_initdata_b16_s *init) +{ + /* Reset data */ + + memset(foc, 0, sizeof(struct foc_data_b16_s)); + + /* Initialize PI current d component */ + + pi_controller_init_b16(&foc->id_pid, init->id_kp, init->id_ki); + + /* Initialize PI current q component */ + + pi_controller_init_b16(&foc->iq_pid, init->iq_kp, init->iq_ki); + + /* Disable PI intergral part reset when saturated */ + + pi_ireset_enable_b16(&foc->iq_pid, false); + pi_ireset_enable_b16(&foc->id_pid, false); + + /* Enable PI anti-windup protection */ + + pi_antiwindup_enable_b16(&foc->iq_pid, ftob16(0.99f), true); + pi_antiwindup_enable_b16(&foc->id_pid, ftob16(0.99f), true); +} + +/**************************************************************************** + * Name: foc_vbase_update_b16 + * + * Description: + * Update base voltage for FOC controller + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * vbase - (in) base voltage for FOC + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_vbase_update_b16(FAR struct foc_data_b16_s *foc, b16_t vbase) +{ + b16_t scale = 0; + b16_t mag_max = 0; + + /* Prevent division by zero */ + + if (vbase < 1) + { + vbase = 1; + } + + /* NOTE: this is base voltage for FOC, not bus voltage! */ + + scale = b16divb16(b16ONE, vbase); + mag_max = vbase; + + /* Update */ + + foc_vab_mod_scale_set_b16(foc, scale); + foc_vdq_mag_max_set_b16(foc, mag_max); +} + +/**************************************************************************** + * Name: foc_angle_update_b16 + * + * Description: + * Update FOC data with new motor phase angle. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * angle - (in) pointer to the phase angle data + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_angle_update_b16(FAR struct foc_data_b16_s *foc, + FAR phase_angle_b16_t *angle) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(angle != NULL); + + /* Copy angle to foc data */ + + foc->angle.angle = angle->angle; + foc->angle.sin = angle->sin; + foc->angle.cos = angle->cos; +} + +/**************************************************************************** + * Name: foc_iabc_update_b16 + * + * Description: + * Update FOC data with new iabc frame. + * + * To work properly this function requires some additional steps: + * 1. phase angle must be set with foc_angle_update() + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * i_abc - (in) pointer to the ABC current frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_iabc_update_b16(FAR struct foc_data_b16_s *foc, + FAR abc_frame_b16_t *i_abc) +{ + dq_frame_b16_t i_dq; + + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(i_abc != NULL); + + /* Reset data */ + + i_dq.d = 0; + i_dq.q = 0; + + /* Copy ABC current to foc data */ + + foc->i_abc.a = i_abc->a; + foc->i_abc.b = i_abc->b; + foc->i_abc.c = i_abc->c; + + /* Convert abc current to alpha-beta current */ + + clarke_transform_b16(&foc->i_abc, &foc->i_ab); + + /* Convert alpha-beta current to dq current */ + + park_transform_b16(&foc->angle, &foc->i_ab, &i_dq); + + /* Store dq current */ + + foc->i_dq.d = i_dq.d; + foc->i_dq.q = i_dq.q; +} + +/**************************************************************************** + * Name: foc_voltage_control_b16 + * + * Description: + * Process FOC voltage control. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * vdq_ref - (in) voltage dq reference frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_voltage_control_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + /* Update VDQ request */ + + foc_vdq_ref_set_b16(foc, vdq_ref); + + /* Inverse Park transform (voltage dq -> voltage alpha-beta) */ + + inv_park_transform_b16(&foc->angle, &foc->v_dq, &foc->v_ab); + +#ifdef CONFIG_LIBDSP_FOC_VABC + /* Inverse Clarke transform (voltage alpha-beta -> voltage abc) */ + + inv_clarke_transform_b16(&foc->v_ab, &foc->v_abc); +#endif + + /* Normalize the alpha-beta voltage to get the alpha-beta modulation + * voltage + */ + + foc->v_ab_mod.a = b16mulb16(foc->v_ab.a, foc->vab_mod_scale); + foc->v_ab_mod.b = b16mulb16(foc->v_ab.b, foc->vab_mod_scale); +} + +/**************************************************************************** + * Name: foc_current_control_b16 + * + * Description: + * Process FOC current control. + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * idq_ref - (in) current dq reference frame + * vdq_comp - (in) voltage dq compensation frame + * vdq_ref - (out) voltage dq reference frame + * + * Returned Value: + * None + * + * TODO: add some reference and a brief description of the FOC + * + ****************************************************************************/ + +void foc_current_control_b16(FAR struct foc_data_b16_s *foc, + FAR dq_frame_b16_t *idq_ref, + FAR dq_frame_b16_t *vdq_comp, + FAR dq_frame_b16_t *vdq_ref) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + /* Update IDQ reference */ + + foc_idq_ref_set_b16(foc, idq_ref); + + /* Run FOC current controler (current dq -> voltage dq) */ + + foc_current_controler_b16(foc, vdq_ref); + + /* DQ voltage compensation */ + + vdq_ref->d = vdq_ref->d - vdq_comp->d; + vdq_ref->q = vdq_ref->q - vdq_comp->q; +} + +/**************************************************************************** + * Name: foc_vabmod_get_b16 + * + * Description: + * Get result from the FOC controler (foc_current_control or + * foc_voltage_control) + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * v_ab_mod - (out) pointer to the voltage alpha-beta modulation frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc, + FAR ab_frame_b16_t *v_ab_mod) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + LIBDSP_DEBUGASSERT(v_ab_mod != NULL); + + v_ab_mod->a = foc->v_ab_mod.a; + v_ab_mod->b = foc->v_ab_mod.b; +} + +/**************************************************************************** + * Name: foc_vdq_mag_max_get_b16 + * + * Description: + * Get maximum dq voltage vector magnitude + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * max - (out) maximum dq voltage magnitude + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max) +{ + LIBDSP_DEBUGASSERT(foc != NULL); + + *max = foc->vdq_mag_max; +} diff --git a/libs/libdsp/lib_misc_b16.c b/libs/libdsp/lib_misc_b16.c new file mode 100644 index 0000000000..66be946735 --- /dev/null +++ b/libs/libdsp/lib_misc_b16.c @@ -0,0 +1,457 @@ +/**************************************************************************** + * libs/libdsp/lib_misc_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define VECTOR2D_SATURATE_MAG_MIN (1) +#define FAST_ATAN2_SMALLNUM (1) + +#ifndef ABS +# define ABS(a) (a < 0 ? -a : a) +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: f_saturate_b16 + * + * Description: + * Saturate b16_t number + * + * Input Parameters: + * val - pointer to b16_t number + * min - lower limit + * max - upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void f_saturate_b16(FAR b16_t *val, b16_t min, b16_t max) +{ + if (*val < min) + { + *val = min; + } + + else if (*val > max) + { + *val = max; + } +} + +/**************************************************************************** + * Name: vector2d_mag_b16 + * + * Description: + * Get 2D vector magnitude. + * + * Input Parameters: + * x - (in) vector x component + * y - (in) vector y component + * + * Returned Value: + * Return 2D vector magnitude + * + ****************************************************************************/ + +b16_t vector2d_mag_b16(b16_t x, b16_t y) +{ + b16_t t0 = 0; + b16_t t1 = 0; + b16_t t3 = 0; + + t0 = b16sqr(x); + t1 = b16sqr(y); + t3 = t0 + t1; + + /* TODO: move to fixedmath sqrt */ + + if (t3 == 0) + { + return 0; + } + +#if CONFIG_LIBDSP_PRECISION == 0 + /* Use ub8 sqrt */ + + return ub8toub16(ub16sqrtub8(t3)); +#else + /* Too slow ! */ + + return ub16sqrtub16(t3); +#endif +} + +/**************************************************************************** + * Name: vector2d_saturate_b16 + * + * Description: + * Saturate 2D vector magnitude. + * + * Input Parameters: + * x - (in/out) pointer to the vector x component + * y - (in/out) pointer to the vector y component + * max - (in) maximum vector magnitude + * + * Returned Value: + * None + * + ****************************************************************************/ + +void vector2d_saturate_b16(FAR b16_t *x, FAR b16_t *y, b16_t max) +{ + b16_t mag = 0; + b16_t tmp = 0; + + /* Get vector magnitude */ + + mag = vector2d_mag_b16(*x, *y); + + if (mag < VECTOR2D_SATURATE_MAG_MIN) + { + mag = VECTOR2D_SATURATE_MAG_MIN; + } + + if (mag > max) + { + /* Saturate vector */ + + tmp = b16divb16(max, mag); + *x = b16mulb16(*x, tmp); + *y = b16mulb16(*x, tmp); + } +} + +/**************************************************************************** + * Name: dq_mag_b16 + * + * Description: + * Get DQ vector magnitude. + * + * Input Parameters: + * dq - (in/out) dq frame vector + * + * Returned Value: + * Return dq vector magnitude + * + ****************************************************************************/ + +b16_t dq_mag_b16(FAR dq_frame_b16_t *dq) +{ + return vector2d_mag_b16(dq->d, dq->q); +} + +/**************************************************************************** + * Name: dq_saturate_b16 + * + * Description: + * Saturate dq frame vector magnitude. + * + * Input Parameters: + * dq - (in/out) dq frame vector + * max - (in) maximum vector magnitude + * + * Returned Value: + * None + * + ****************************************************************************/ + +void dq_saturate_b16(FAR dq_frame_b16_t *dq, b16_t max) +{ + vector2d_saturate_b16(&dq->d, &dq->q, max); +} + +/**************************************************************************** + * Name: fast_sin_b16 + * + * Description: + * Fast sin calculation + * + * Reference: http://lab.polygonal.de/?p=205 + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated sine value + * + ****************************************************************************/ + +b16_t fast_sin_b16(b16_t angle) +{ + b16_t sin = 0; + b16_t n1 = ftob16(1.27323954f); + b16_t n2 = ftob16(0.405284735f); + b16_t tmp1 = 0; + b16_t tmp2 = 0; + + /* Normalize angle */ + + angle_norm_2pi_b16(&angle, -b16PI, b16PI); + + /* Get estiamte sine value from quadratic equation */ + + if (angle < 0) + { + tmp1 = b16mulb16(n1, angle); + tmp2 = b16mulb16(n2, b16sqr(angle)); + sin = tmp1 + tmp2; + } + else + { + tmp1 = b16mulb16(n1, angle); + tmp2 = b16mulb16(n2, b16sqr(angle)); + sin = tmp1 - tmp2; + } + + return sin; +} + +/**************************************************************************** + * Name:fast_cos_b16 + * + * Description: + * Fast cos calculation + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated cosine value + * + ****************************************************************************/ + +b16_t fast_cos_b16(b16_t angle) +{ + /* Get cosine value from sine sin(x + PI/2) = cos(x) */ + + return fast_sin_b16(angle + b16HALFPI); +} + +/**************************************************************************** + * Name: fast_sin2_b16 + * + * Description: + * Fast sin calculation with better accuracy + * + * Reference: http://lab.polygonal.de/?p=205 + * + * Input Parameters: + * angle + * + * Returned Value: + * Return estimated sine value + * + ****************************************************************************/ + +b16_t fast_sin2_b16(b16_t angle) +{ + return b16sin(angle); +} + +/**************************************************************************** + * Name:fast_cos2_b16 + * + * Description: + * Fast cos calculation with better accuracy + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated cosine value + * + ****************************************************************************/ + +b16_t fast_cos2_b16(b16_t angle) +{ + return b16cos(angle); +} + +/**************************************************************************** + * Name: fast_atan2_b16 + * + * Description: + * Fast atan2 calculation + * + * REFERENCE: + * https://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization/ + * + * Input Parameters: + * x - (in) + * y - (in) + * + * Returned Value: + * Return estimated angle + * + ****************************************************************************/ + +b16_t fast_atan2_b16(b16_t y, b16_t x) +{ + b16_t angle = 0; + b16_t abs_y = 0; + b16_t rsq = 0; + b16_t r = 0; + b16_t n1 = ftob16(0.1963f); + b16_t n2 = ftob16(0.9817f); + b16_t tmp1 = 0; + b16_t tmp2 = 0; + b16_t tmp3 = 0; + + /* Get absolute value of y and add some small number to prevent 0/0 */ + + abs_y = ABS(y) + FAST_ATAN2_SMALLNUM; + + /* Calculate angle */ + + if (x >= 0) + { + r = b16divb16((x - abs_y), (x + abs_y)); + rsq = b16mulb16(r, r); + tmp1 = b16mulb16(n1, rsq); + tmp2 = b16mulb16((tmp1 - n2), r); + tmp3 = b16mulb16(b16PI, ftob16(0.25f)); + angle = tmp2 + tmp3; + } + else + { + r = b16divb16((x + abs_y), (abs_y - x)); + rsq = b16mulb16(r, r); + tmp1 = b16mulb16(n1, rsq); + tmp2 = b16mulb16((tmp1 - n2), r); + tmp3 = b16mulb16(b16PI, ftob16(0.75f)); + angle = tmp2 + tmp3; + } + + /* Get angle sign */ + + if (y < 0) + { + angle = -angle; + } + + return angle; +} + +/**************************************************************************** + * Name: angle_norm_b16 + * + * Description: + * Normalize radians angle to a given boundary and a given period. + * + * Input Parameters: + * angle - (in/out) pointer to the angle data + * per - (in) angle period + * bottom - (in) lower limit + * top - (in) upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void angle_norm_b16(FAR b16_t *angle, b16_t per, b16_t bottom, b16_t top) +{ + while (*angle > top) + { + /* Move the angle backwards by given period */ + + *angle = *angle - per; + } + + while (*angle < bottom) + { + /* Move the angle forwards by given period */ + + *angle = *angle + per; + } +} + +/**************************************************************************** + * Name: angle_norm_2pi_b16 + * + * Description: + * Normalize radians angle with period 2*PI to a given boundary. + * + * Input Parameters: + * angle - (in/out) pointer to the angle data + * bottom - (in) lower limit + * top - (in) upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void angle_norm_2pi_b16(FAR b16_t *angle, b16_t bottom, b16_t top) +{ + angle_norm_b16(angle, b16TWOPI, bottom, top); +} + +/**************************************************************************** + * Name: phase_angle_update_b16 + * + * Description: + * Update phase_angle_s structure: + * 1. normalize angle value to <0.0, 2PI> range + * 2. update angle value + * 3. update sin/cos value for given angle + * + * Input Parameters: + * angle - (in/out) pointer to the angle data + * val - (in) angle radian value + * + * Returned Value: + * None + * + ****************************************************************************/ + +void phase_angle_update_b16(FAR struct phase_angle_b16_s *angle, b16_t val) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + + /* Normalize angle to <0.0, 2PI> */ + + angle_norm_2pi_b16(&val, 0, b16TWOPI); + + /* Update structure */ + + angle->angle = val; + +#if CONFIG_LIBDSP_PRECISION == 0 + angle->sin = fast_sin_b16(val); + angle->cos = fast_cos_b16(val); +#else + angle->sin = fast_sin2_b16(val); + angle->cos = fast_cos2_b16(val); +#endif +} diff --git a/libs/libdsp/lib_motor_b16.c b/libs/libdsp/lib_motor_b16.c new file mode 100644 index 0000000000..899cd411c9 --- /dev/null +++ b/libs/libdsp/lib_motor_b16.c @@ -0,0 +1,279 @@ +/**************************************************************************** + * control/lib_motor_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define POLE_CNTR_THR (0) + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: motor_angle_e_update_b16 + * + * Description: + * Update motor angle structure using electrical motor angle. + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * angle_new - (in) new motor electrical angle in range <0.0, 2PI> + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_angle_e_update_b16(FAR struct motor_angle_b16_s *angle, + b16_t angle_new, b16_t dir) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16); + LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16); + + b16_t tmp1 = 0; + b16_t tmp2 = 0; + + /* Check if we crossed electrical angle boundaries */ + + if (dir == DIR_CW_B16) + { + /* For CW direction - previous angle is greater than current angle */ + + if (angle_new - angle->angle_el.angle < -POLE_CNTR_THR) + { + angle->i += 1; + } + } + + else if (dir == DIR_CCW_B16) + { + /* For CCW direction - previous angle is lower than current angle */ + + if (angle_new - angle->angle_el.angle > POLE_CNTR_THR) + { + angle->i -= 1; + } + } + + /* Reset pole counter if needed */ + + if (angle->i >= angle->p) + { + angle->i = 0; + } + + else if (angle->i < 0) + { + angle->i = angle->p - 1; + } + + /* Update electrical angle structure */ + + phase_angle_update_b16(&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. + */ + + tmp1 = b16mulb16(MOTOR_ANGLE_E_RANGE_B16, itob16(angle->i)); + tmp2 = tmp1 + angle->angle_el.angle; + + angle->anglem = b16mulb16(tmp2, angle->one_by_p); + + /* Normalize mechanical angle to <0, 2PI> and store */ + + angle_norm_2pi_b16(&angle->anglem, MOTOR_ANGLE_M_MIN_B16, + MOTOR_ANGLE_M_MAX_B16); +} + +/**************************************************************************** + * Name: motor_angle_m_update_b16 + * + * Description: + * Update motor angle structure using mechanical motor angle + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * angle_new - (in) new motor mechanical angle in range <0.0, 2PI> + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_angle_m_update_b16(FAR struct motor_angle_b16_s *angle, + b16_t angle_new, b16_t dir) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16); + LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16); + + b16_t angle_el = 0; + b16_t tmp1 = 0; + b16_t tmp2 = 0; + + /* Store new mechanical angle */ + + angle->anglem = angle_new; + + /* Update pole counter */ + + tmp1 = b16mulb16(angle->anglem, itob16(angle->p)); + + angle->i = (uint8_t) b16toi(b16divb16(tmp1, MOTOR_ANGLE_M_MAX_B16)); + + /* Get electrical angle */ + + tmp1 = b16mulb16(angle->anglem, itob16(angle->p)); + tmp2 = b16mulb16(MOTOR_ANGLE_E_MAX_B16, itob16(angle->i)); + + angle_el = (tmp1 - tmp2); + + /* Update electrical angle structure */ + + phase_angle_update_b16(&angle->angle_el, angle_el); +} + +/**************************************************************************** + * Name: motor_angle_m_get_b16 + * + * Description: + * Get motor mechanical angle + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * + * Returned Value: + * Return motor mechanical angle + * + ****************************************************************************/ + +b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + + return angle->anglem; +} + +/**************************************************************************** + * Name: motor_angle_e_get_b16 + * + * Description: + * Get motor electrical angle + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * + * Returned Value: + * Return motor electrical angle + * + ****************************************************************************/ + +b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + + return angle->angle_el.angle; +} + +/**************************************************************************** + * Name: motor_phy_params_init_b16 + * + * Description: + * Initialize motor physical parameters + * + * Input Parameters: + * phy - (in/out) pointer to the motor physical parameters + * poles - (in) number of the motor pole pairs + * res - (in) average phase-to-neutral base motor resistance + * (without temperature compensation) + * ind - (in) average phase-to-neutral motor inductance + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_phy_params_init_b16(FAR struct motor_phy_params_b16_s *phy, + uint8_t poles, b16_t res, b16_t ind) +{ + LIBDSP_DEBUGASSERT(phy != NULL); + + memset(phy, 0, sizeof(struct motor_phy_params_b16_s)); + + phy->p = poles; + phy->res = res; + phy->ind = ind; + phy->one_by_ind = b16divb16(b16ONE, ind); +} + +/**************************************************************************** + * Name: pmsm_phy_params_init_b16 + * + * Description: + * Initialize PMSM physical parameters + * + * Input Parameters: + * phy - (in/out) pointer to the PMSM physical parameters + * poles - (in) number of the motor pole pairs + * res - (in) average phase-to-neutral base motor resistance + * (without temperature compensation) + * ind - (in) average phase-to-neutral motor inductance + * iner - (in) rotor inertia (J) + * flux - (in) flux linkage + * ind_d - (in) d-inductance + * ind_q - (in) q-inductance + * + * Returned Value: + * None + * + ****************************************************************************/ + +void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy, + uint8_t poles, b16_t res, b16_t ind, + b16_t iner, b16_t flux, + b16_t ind_d, b16_t ind_q) +{ + LIBDSP_DEBUGASSERT(phy != NULL); + + /* Initialize motor phy */ + + motor_phy_params_init_b16(&phy->motor, poles, res, ind); + + /* Iniitalize PMSM specific data */ + + phy->iner = iner; + phy->flux_link = flux; + phy->ind_d = ind_d; + phy->ind_q = ind_q; + phy->one_by_iner = b16divb16(b16ONE, iner); + phy->one_by_indd = b16divb16(b16ONE, ind_d); + phy->one_by_indq = b16divb16(b16ONE, ind_q); +} diff --git a/libs/libdsp/lib_pid_b16.c b/libs/libdsp/lib_pid_b16.c new file mode 100644 index 0000000000..ec68e14d19 --- /dev/null +++ b/libs/libdsp/lib_pid_b16.c @@ -0,0 +1,352 @@ +/**************************************************************************** + * libs/libdsp/lib_pid_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pid_controller_init_b16 + * + * Description: + * Initialize PID controller. This function does not initialize saturation + * limits. + * + * Input Parameters: + * pid - (out) pointer to the PID controller data + * KP - (in) proportional gain + * KI - (in) integral gain + * KD - (in) derivative gain + * + * Returned Value: + * None + * + ****************************************************************************/ + +void pid_controller_init_b16(FAR pid_controller_b16_t *pid, b16_t KP, + b16_t KI, b16_t KD) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + + /* Reset controller data */ + + memset(pid, 0, sizeof(pid_controller_b16_t)); + + /* Copy controller parameters */ + + pid->KP = KP; + pid->KI = KI; + pid->KD = KD; + pid->KC = 0; +} + +/**************************************************************************** + * Name: pi_controller_init_b16 + * + * Description: + * Initialize PI controller. This function does not initialize saturation + * limits. + * + * Input Parameters: + * pid - (out) pointer to the PID controller data + * KP - (in) proportional gain + * KI - (in) integral gain + * + * Returned Value: + * None + * + ****************************************************************************/ + +void pi_controller_init_b16(FAR pid_controller_b16_t *pid, b16_t KP, + b16_t KI) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + + /* Reset controller data */ + + memset(pid, 0, sizeof(pid_controller_b16_t)); + + /* Copy controller parameters */ + + pid->KP = KP; + pid->KI = KI; + pid->KD = 0; + pid->KC = 0; + + /* No windup-protection at default */ + + pid->aw_en = false; + pid->ireset_en = false; +} + +/**************************************************************************** + * Name: pid_saturation_set_b16 + * + * Description: + * Set controller saturation limits. Sometimes we need change saturation + * configuration in the run-time, so this function is separate from + * pid_controller_init(). + * + * Input Parameters: + * pid - (out) pointer to the PID controller data + * min - (in) lower limit + * max - (in) upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void pid_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min, + b16_t max) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + LIBDSP_DEBUGASSERT(min < max); + + pid->sat.max = max; + pid->sat.min = min; + + /* Enable saturation in PID controller */ + + pid->pidsat_en = true; +} + +/**************************************************************************** + * Name: pi_saturation_set_b16 + * + * Description: + * + * Input Parameters: + * pid - (out) pointer to the PID controller data + * min - (in) lower limit + * max - (in) upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void pi_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min, + b16_t max) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + LIBDSP_DEBUGASSERT(min < max); + + pid->sat.max = max; + pid->sat.min = min; + + /* Enable saturation in PI controller */ + + pid->pisat_en = true; +} + +/**************************************************************************** + * Name: pid_antiwindup_enable_b16 + ****************************************************************************/ + +void pi_antiwindup_enable_b16(FAR pid_controller_b16_t *pid, b16_t KC, + bool enable) +{ + pid->aw_en = enable; + pid->KC = KC; +} + +/**************************************************************************** + * Name: pid_ireset_enable_b16 + ****************************************************************************/ + +void pi_ireset_enable_b16(FAR pid_controller_b16_t *pid, bool enable) +{ + pid->ireset_en = enable; +} + +/**************************************************************************** + * Name: pid_integral_reset_b16 + ****************************************************************************/ + +void pid_integral_reset_b16(FAR pid_controller_b16_t *pid) +{ + pid->part[1] = 0; + pid->aw = 0; +} + +/**************************************************************************** + * Name: pi_integral_reset_b16 + ****************************************************************************/ + +void pi_integral_reset_b16(FAR pid_controller_b16_t *pid) +{ + pid_integral_reset_b16(pid); +} + +/**************************************************************************** + * Name: pi_controller_b16 + * + * Description: + * PI controller with output saturation and windup protection + * + * Input Parameters: + * pid - (in/out) pointer to the PI controller data + * err - (in) current controller error + * + * Returned Value: + * Return controller output. + * + ****************************************************************************/ + +b16_t pi_controller_b16(FAR pid_controller_b16_t *pid, b16_t err) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + + b16_t tmp = 0; + + /* Store error in controller structure */ + + pid->err = err; + + /* Get proportional part */ + + pid->part[0] = b16mulb16(pid->KP, err); + + /* Get intergral part */ + + pid->part[1] += b16mulb16(pid->KI, (err - pid->aw)); + + /* Add proportional, integral */ + + pid->out = pid->part[0] + pid->part[1]; + + /* Store not saturated output */ + + tmp = pid->out; + + /* Saturate output if enabled */ + + if (pid->pisat_en == true) + { + if (pid->out > pid->sat.max) + { + if (pid->ireset_en == true) + { + /* Reset I part */ + + if (err > 0) + { + pi_integral_reset_b16(pid); + } + } + + /* Limit output to the upper limit */ + + pid->out = pid->sat.max; + } + else if (pid->out < pid->sat.min) + { + if (pid->ireset_en == true) + { + /* Reset I part */ + + if (err < 0) + { + pi_integral_reset_b16(pid); + } + } + + /* Limit output to the lower limit */ + + pid->out = pid->sat.min; + } + } + + /* Anti-windup I-part decay if enabled */ + + if (pid->aw_en == true) + { + pid->aw = b16mulb16(pid->KC, (tmp - pid->out)); + } + + /* Return regulator output */ + + return pid->out; +} + +/**************************************************************************** + * Name: pid_controller_b16 + * + * Description: + * PID controller with output saturation and windup protection + * + * Input Parameters: + * pid - (in/out) pointer to the PID controller data + * err - (in) current controller error + * + * Returned Value: + * Return controller output. + * + ****************************************************************************/ + +b16_t pid_controller_b16(FAR pid_controller_b16_t *pid, b16_t err) +{ + LIBDSP_DEBUGASSERT(pid != NULL); + + /* Get PI output */ + + pi_controller_b16(pid, err); + + /* Get derivative part */ + + pid->part[2] = b16mulb16(pid->KD, (err - pid->err_prev)); + + /* Add derivative part to the PI part */ + + pid->out += pid->part[2]; + + /* Store current error */ + + pid->err_prev = err; + + /* Saturate output if enabled */ + + if (pid->pidsat_en == true) + { + if (pid->out > pid->sat.max) + { + /* Limit output to the upper limit */ + + pid->out = pid->sat.max; + } + else if (pid->out < pid->sat.min) + { + /* Limit output to the lower limit */ + + pid->out = pid->sat.min; + } + } + + /* Return regulator output */ + + return pid->out; +} diff --git a/libs/libdsp/lib_pmsm_model_b16.c b/libs/libdsp/lib_pmsm_model_b16.c new file mode 100644 index 0000000000..2ef4ec5e5f --- /dev/null +++ b/libs/libdsp/lib_pmsm_model_b16.c @@ -0,0 +1,197 @@ +/**************************************************************************** + * libs/libdsp/lib_pmsm_model_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pmsm_model_initialize_b16 + * + * Description: + * Initialzie FOC model + * + ****************************************************************************/ + +int pmsm_model_initialize_b16(FAR struct pmsm_model_b16_s *model, + FAR struct pmsm_phy_params_b16_s *phy, + b16_t per) +{ + DEBUGASSERT(model); + DEBUGASSERT(phy); + DEBUGASSERT(per > 0); + + /* Copy motor model parameters */ + + memcpy(&model->phy, phy, sizeof(struct pmsm_phy_params_b16_s)); + + /* Initialize controller period */ + + model->per = per; + + return OK; +} + +/**************************************************************************** + * Name: pmsm_model_elec_b16 + * + * Description: + * Update motor model electrical state + * + ****************************************************************************/ + +int pmsm_model_elec_b16(FAR struct pmsm_model_b16_s *model, + FAR ab_frame_b16_t *vab) +{ + b16_t tmp1 = 0; + b16_t tmp2 = 0; + b16_t tmp3 = 0; + b16_t tmp4 = 0; + b16_t tmp5 = 0; + b16_t tmp6 = 0; + + DEBUGASSERT(model); + DEBUGASSERT(vab); + + /* Copy alpha-beta voltage */ + + model->state.v_ab.a = vab->a; + model->state.v_ab.b = vab->b; + + /* Inverse Clarke transform - get abc voltage */ + + inv_clarke_transform_b16(&model->state.v_ab, + &model->state.v_abc); + + /* Park transform - get DQ voltage */ + + park_transform_b16(&model->state.angle.angle_el, + &model->state.v_ab, + &model->state.v_dq); + + /* q current */ + + tmp1 = b16mulb16(model->phy.motor.res, model->state.i_dq.q); + tmp2 = b16mulb16(model->phy.ind_d, model->state.i_dq.d); + tmp3 = tmp2 + model->phy.flux_link; + tmp4 = b16mulb16(model->state.omega_e, tmp3); + tmp5 = model->state.v_dq.q - tmp1 - tmp4; + tmp6 = b16mulb16(model->per, tmp5); + + model->iq_int += b16mulb16(tmp6, model->phy.one_by_indq); + + /* d current */ + + tmp1 = b16mulb16(model->phy.motor.res, model->state.i_dq.d); + tmp2 = b16mulb16(model->phy.ind_q, model->state.i_dq.q); + tmp3 = b16mulb16(tmp2, model->state.omega_e); + tmp4 = model->state.v_dq.d - tmp1 + tmp3; + tmp5 = b16mulb16(model->per, tmp4); + + model->id_int += b16mulb16(tmp5, model->phy.one_by_indd); + + /* Store currents */ + + model->state.i_dq.q = model->iq_int; + model->state.i_dq.d = model->id_int; + + /* Inverse Park transform - get alpha-beta current */ + + inv_park_transform_b16(&model->state.angle.angle_el, + &model->state.i_dq, + &model->state.i_ab); + + /* Inverse Clarke transform - get abc current */ + + inv_clarke_transform_b16(&model->state.i_ab, + &model->state.i_abc); + + return OK; +} + +/**************************************************************************** + * Name: pmsm_model_mech_b16 + * + * Description: + * Update motor model mechanical state + * + ****************************************************************************/ + +int pmsm_model_mech_b16(FAR struct pmsm_model_b16_s *model, b16_t load) +{ + b16_t angle = 0; + b16_t dir = 0; + b16_t te = 0; + b16_t tmp1 = 0; + b16_t tmp2 = 0; + b16_t tmp3 = 0; + b16_t tmp4 = 0; + b16_t tmp5 = 0; + + DEBUGASSERT(model); + + /* Get electrical torque developed by the motor */ + + tmp1 = model->phy.ind_d - model->phy.ind_q; + tmp2 = b16mulb16(tmp1, model->state.i_dq.d); + tmp3 = model->phy.flux_link - tmp2; + tmp4 = b16mulb16((b16ONE + b16HALF), itob16(model->phy.motor.p)); + tmp5 = b16mulb16(tmp4, model->state.i_dq.q); + + te = b16mulb16(tmp5, tmp3); + + /* Get new mechanical velocity */ + + tmp1 = te - load; + tmp2 = b16mulb16(model->per, tmp1); + tmp3 = b16mulb16(tmp2, model->phy.one_by_iner); + + model->state.omega_m = model->state.omega_m + tmp3; + + /* Get new electrical velocity */ + + model->state.omega_e = b16mulb16(model->state.omega_m, + itob16(model->phy.motor.p)); + + /* Get rotation direction */ + + dir = (model->state.omega_e > 0 ? DIR_CW_B16 : DIR_CCW_B16); + + /* Update electrical angle */ + + tmp1 = b16mulb16(model->state.omega_e, model->per); + + angle = model->state.angle.angle_el.angle + tmp1; + + /* Update with mechanical angel */ + + motor_angle_e_update_b16(&model->state.angle, angle, dir); + + return OK; +} diff --git a/libs/libdsp/lib_svm_b16.c b/libs/libdsp/lib_svm_b16.c new file mode 100644 index 0000000000..56fdafdc16 --- /dev/null +++ b/libs/libdsp/lib_svm_b16.c @@ -0,0 +1,437 @@ +/**************************************************************************** + * libs/libdsp/lib_svm_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: svm3_sector_get_b16 + * + * Description: + * Get current sector for space vector modulation. + * + * Input Parameters: + * ijk - (in) pointer to the auxiliary ABC frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +static uint8_t svm3_sector_get_b16(FAR abc_frame_b16_t *ijk) +{ + uint8_t sector = 0; + b16_t i = ijk->a; + b16_t j = ijk->b; + b16_t k = ijk->c; + + /* Identify the correct sector based on i,j,k frame: + * 1. sector 1: + * i > 0.0 + * j > 0.0 + * k <= 0.0 + * 2. sector 2: + * i <= 0.0 + * j > 0.0 + * k <= 0.0 + * 3. sector 3: + * i <= 0.0 + * j > 0.0 + * k > 0.0 + * 4. sector 4: + * i <= 0.0 + * j <= 0.0 + * k > 0.0 + * 5. sector 5: + * i > 0.0 + * j <= 0.0 + * k > 0.0 + * 6. sector 6: + * i > 0.0 + * j <= 0.0 + * k <= 0.0 + */ + + if (k <= 0) + { + if (i <= 0) + { + sector = 2; + } + else + { + if (j <= 0) + { + sector = 6; + } + else + { + sector = 1; + } + } + } + else + { + if (i <= 0) + { + if (j <= 0) + { + sector = 4; + } + else + { + sector = 3; + } + } + else + { + sector = 5; + } + } + + /* Return SVM sector */ + + return sector; +} + +/**************************************************************************** + * Name: svm3_duty_calc_b16 + * + * Description: + * Calculate duty cycles for space vector modulation. + * + * Input Parameters: + * s - (in/out) pointer to the SVM state data + * ijk - (in) pointer to the auxiliary ABC frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +static void svm3_duty_calc_b16(FAR struct svm3_state_b16_s *s, + FAR abc_frame_b16_t *ijk) +{ + b16_t i = ijk->a; + b16_t j = ijk->b; + b16_t k = ijk->c; + b16_t T0 = 0; + b16_t T1 = 0; + b16_t T2 = 0; + + /* Determine T1, T2 and T0 based on the sector */ + + switch (s->sector) + { + case 1: + { + T1 = i; + 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 */ + + LIBDSP_DEBUGASSERT(0); + break; + } + } + + /* Get null vector time */ + + T0 = b16ONE - T1 - T2; + + /* Calculate duty cycle for 3 phase */ + + switch (s->sector) + { + case 1: + { + s->d_u = T1 + T2 + b16mulb16(T0, b16HALF); + s->d_v = T2 + b16mulb16(T0, b16HALF); + s->d_w = b16mulb16(T0, b16HALF); + break; + } + + case 2: + { + s->d_u = T1 + b16mulb16(T0, b16HALF); + s->d_v = T1 + T2 + b16mulb16(T0, b16HALF); + s->d_w = b16mulb16(T0, b16HALF); + break; + } + + case 3: + { + s->d_u = b16mulb16(T0, b16HALF); + s->d_v = T1 + T2 + b16mulb16(T0, b16HALF); + s->d_w = T2 + b16mulb16(T0, b16HALF); + break; + } + + case 4: + { + s->d_u = b16mulb16(T0, b16HALF); + s->d_v = T1 + b16mulb16(T0, b16HALF); + s->d_w = T1 + T2 + b16mulb16(T0, b16HALF); + break; + } + + case 5: + { + s->d_u = T2 + b16mulb16(T0, b16HALF); + s->d_v = b16mulb16(T0, b16HALF); + s->d_w = T1 + T2 + b16mulb16(T0, b16HALF); + break; + } + + case 6: + { + s->d_u = T1 + T2 + b16mulb16(T0, b16HALF); + s->d_v = b16mulb16(T0, b16HALF); + s->d_w = T1 + b16mulb16(T0, b16HALF); + break; + } + + default: + { + /* We should not get here */ + + LIBDSP_DEBUGASSERT(0); + break; + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: svm3_b16 + * + * Description: + * One step of the space vector modulation. + * This is most common of SVM with alternate-reverse null vector. + * + * Voltage vector definitions in 3-phase SVM: + * + * |---------|-----------|--------------------|-----------------| + * | Voltage | swithcing | Line to neutral | Line to line | + * | vector | vectors | voltage | voltage | + * | |-----------|--------------------|-----------------| + * | | a | b | c | Van | Vbn | Vcn | Vab | Vbe | Vca | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V1 | 1 | 0 | 0 | 2/3 | -1/3 | -1/3 | 1 | 0 | -1 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V2 | 1 | 1 | 0 | 1/3 | 1/3 | -2/3 | 0 | 1 | -1 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V3 | 0 | 1 | 0 | -1/3 | 2/3 | -1/3 | -1 | 1 | 0 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V4 | 0 | 1 | 1 | -2/3 | 1/3 | 1/3 | -1 | 0 | 1 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V5 | 0 | 0 | 1 | -1/3 | -1/3 | 2/3 | 0 | -1 | 1 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V6 | 1 | 0 | 1 | 1/3 | -2/3 | 1/3 | 1 | -1 | 0 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * | V7 | 1 | 1 | 1 | 0 | 0 | 0 | 0 | 0 | 0 | + * |---------|---|---|---|------|------|------|-----|-----|-----| + * + * Voltage values given in relation to the bus voltage (Vbus)/ + * + * 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) + * + * NOTE: v_ab vector magnitude must be in range <0.0, 1.0> to get correct + * SVM3 results. + * + * REFERENCE: + * https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download + * pages 32-34 + * + ****************************************************************************/ + +void svm3_b16(FAR struct svm3_state_b16_s *s, FAR ab_frame_b16_t *v_ab) +{ + LIBDSP_DEBUGASSERT(s != NULL); + LIBDSP_DEBUGASSERT(v_ab != NULL); + + abc_frame_b16_t ijk; + + /* Perform modified inverse Clarke-transformation (alpha,beta) -> (i,j,k) + * to obtain auxiliary frame which will be used in further calculations. + */ + + ijk.a = b16mulb16(-b16HALF, v_ab->b) + b16mulb16(SQRT3_BY_TWO_B16, + v_ab->a); + ijk.b = v_ab->b; + ijk.c = -ijk.b - ijk.a; + + /* Get vector sector */ + + s->sector = svm3_sector_get_b16(&ijk); + + /* Get duty cycle */ + + svm3_duty_calc_b16(s, &ijk); + + /* NOTE: we return not-saturated output. Duty-cycle saturation is + * board-specific characteristic and we have not access to this + * information here. + */ +} + +/**************************************************************************** + * Name: svm3_current_correct_b16 + * + * Description: + * Correct ADC samples (int32) according to SVM3 state. + * NOTE: This works only with 3 shunt resistors configuration. + * + ****************************************************************************/ + +void svm3_current_correct_b16(FAR struct svm3_state_b16_s *s, + b16_t *c0, b16_t *c1, b16_t *c2) +{ + /* Get best ADC samples according to SVM sector. + * + * In SVM phase current can be sampled only in v0 vector state, when lower + * bridge transistors are turned on. + * + * We ignore sample from phase which has the shortest V0 state and + * estimate its value with KCL for motor phases: + * i_a + i_b + i_c = 0 + */ + + switch (s->sector) + { + case 1: + case 6: + { + /* Sector 1-6: ignore phase 1 */ + + *c0 = -(*c1 + *c2); + + break; + } + + case 2: + case 3: + { + /* Sector 2-3: ignore phase 2 */ + + *c1 = -(*c0 + *c2); + + break; + } + + case 4: + case 5: + { + /* Sector 4-5: ignore phase 3 */ + + *c2 = -(*c0 + *c1); + + break; + } + + default: + { + /* We should not get here. */ + + *c0 = 0; + *c1 = 0; + *c2 = 0; + + break; + } + } +} + +/**************************************************************************** + * Name: svm3_init_b16 + * + * Description: + * Initialize 3-phase SVM data. + * + * Input Parameters: + * s - (in/out) pointer to the SVM state data + * + * Returned Value: + * None + * + ****************************************************************************/ + +void svm3_init_b16(FAR struct svm3_state_b16_s *s) +{ + LIBDSP_DEBUGASSERT(s != NULL); + + memset(s, 0, sizeof(struct svm3_state_b16_s)); +} diff --git a/libs/libdsp/lib_transform_b16.c b/libs/libdsp/lib_transform_b16.c new file mode 100644 index 0000000000..6fc663c47e --- /dev/null +++ b/libs/libdsp/lib_transform_b16.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * libs/libdsp/lib_transform_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: clarke_transform_b16 + * + * Description: + * Clarke transform (abc frame -> ab frame). + * Transform the abc frame to the alpha-beta frame. + * + * i_alpha = k*(i_a - 0.5*i_b - 0.5*i_c) + * i_beta = k*sqrt(3)*0.5*(i_b - i_c) + * + * We assume that: + * 1) k = 2/3 for the non-power-invariant transformation + * 2) balanced system: a + b + c = 0 + * + * Input Parameters: + * abc - (in) pointer to the abc frame + * ab - (out) pointer to the alpha-beta frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void clarke_transform_b16(FAR abc_frame_b16_t *abc, + FAR ab_frame_b16_t *ab) +{ + LIBDSP_DEBUGASSERT(abc != NULL); + LIBDSP_DEBUGASSERT(ab != NULL); + + ab->a = abc->a; + ab->b = (b16mulb16(ONE_BY_SQRT3_B16, abc->a) + + b16mulb16(TWO_BY_SQRT3_B16, abc->b)); +} + +/**************************************************************************** + * Name: inv_clarke_transform_b16 + * + * Description: + * Inverse Clarke transform (ab frame -> abc frame). + * Transform the alpha-beta frame to the abc frame. + * + * Input Parameters: + * ab - (in) pointer to the alpha-beta frame + * abc - (out) pointer to the abc frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void inv_clarke_transform_b16(FAR ab_frame_b16_t *ab, + FAR abc_frame_b16_t *abc) +{ + LIBDSP_DEBUGASSERT(ab != NULL); + LIBDSP_DEBUGASSERT(abc != NULL); + + /* Assume non-power-invariant transform and balanced system */ + + abc->a = ab->a; + abc->b = (b16mulb16(-b16HALF, ab->a) + + b16mulb16(SQRT3_BY_TWO_B16, ab->b)); + abc->c = (-abc->a - abc->b); +} + +/**************************************************************************** + * Name: park_transform_b16 + * + * Description: + * Park transform (ab frame -> dq frame). + * Transform the alpha-beta frame to the direct-quadrature frame. + * + * Input Parameters: + * angle - (in) pointer to the phase angle data + * ab - (in) pointer to the alpha-beta frame + * dq - (out) pointer to the direct-quadrature frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void park_transform_b16(FAR phase_angle_b16_t *angle, + FAR ab_frame_b16_t *ab, + FAR dq_frame_b16_t *dq) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + LIBDSP_DEBUGASSERT(ab != NULL); + LIBDSP_DEBUGASSERT(dq != NULL); + + dq->d = b16mulb16(angle->cos, ab->a) + b16mulb16(angle->sin, ab->b); + dq->q = b16mulb16(angle->cos, ab->b) - b16mulb16(angle->sin, ab->a); +} + +/**************************************************************************** + * Name: inv_park_transform_b16 + * + * Description: + * Inverse Park transform (dq frame -> ab frame). + * Transform direct-quadrature frame to alpha-beta frame. + * + * Input Parameters: + * angle - (in) pointer to the phase angle data + * dq - (in) pointer to the direct-quadrature frame + * ab - (out) pointer to the alpha-beta frame + * + * Returned Value: + * None + * + ****************************************************************************/ + +void inv_park_transform_b16(FAR phase_angle_b16_t *angle, + FAR dq_frame_b16_t *dq, + FAR ab_frame_b16_t *ab) +{ + LIBDSP_DEBUGASSERT(angle != NULL); + LIBDSP_DEBUGASSERT(dq != NULL); + LIBDSP_DEBUGASSERT(ab != NULL); + + ab->a = b16mulb16(angle->cos, dq->d) - b16mulb16(angle->sin, dq->q); + ab->b = b16mulb16(angle->cos, dq->q) + b16mulb16(angle->sin, dq->d); +}