From 8416d9a96688ea683a102101a4c0e109b45b1b96 Mon Sep 17 00:00:00 2001 From: Mateusz Szafoni Date: Sat, 7 Jul 2018 17:04:57 +0000 Subject: [PATCH] Merged in raiden00/nuttx (pull request #668) Master * libdsp: initial commit * libdsp: cosmetics * stm32f334-disco/buckboost: use a PID controller from libdsp * stm32_adc.h: fix typo * stm32_dac.c: set OUTEN bit for DAC1CH2 and DAC2CH1 * stm32_hrtim: cosmetic changes * power/motor: direction parameter is now int8 + add overload fault * libdsp: all floats with f-sufix libdsp: add precision option for library libdsp: add debug option for library and assertions in functions libdsp: add current samples correction for SVM3 libds: add some motor control specific functions libdsp: add basic speed observer libdsp: fix phase shift in SMO observer libdsp: add more logic to FOC config/sim/dsptest: add dsptest configuration * libdsp/lib_motor.c: remove unused comparation * libdsp/lib_observer.c: update some comments Approved-by: GregoryN --- arch/arm/src/stm32/stm32_adc.h | 4 +- arch/arm/src/stm32/stm32_dac.c | 15 +- arch/arm/src/stm32/stm32_hrtim.c | 2 +- arch/arm/src/stm32/stm32_hrtim.h | 2 + configs/sim/dsptest/defconfig | 33 +++ drivers/power/motor.c | 14 + include/dsp.h | 255 ++++++++++++++++-- include/nuttx/power/motor.h | 21 +- libs/libdsp/Kconfig | 20 ++ libs/libdsp/Makefile | 1 + libs/libdsp/lib_foc.c | 236 +++++++++++++++-- libs/libdsp/lib_misc.c | 108 +++++--- libs/libdsp/lib_motor.c | 426 +++++++++++++++++++++++++++++++ libs/libdsp/lib_observer.c | 312 ++++++++++++++++++---- libs/libdsp/lib_pid.c | 55 +++- libs/libdsp/lib_svm.c | 166 ++++++++++-- libs/libdsp/lib_transform.c | 16 +- 17 files changed, 1507 insertions(+), 179 deletions(-) create mode 100644 configs/sim/dsptest/defconfig create mode 100644 libs/libdsp/lib_motor.c diff --git a/arch/arm/src/stm32/stm32_adc.h b/arch/arm/src/stm32/stm32_adc.h index e8c5b29773..b77113be16 100644 --- a/arch/arm/src/stm32/stm32_adc.h +++ b/arch/arm/src/stm32/stm32_adc.h @@ -1464,7 +1464,7 @@ #if defined(CONFIG_STM32_HRTIM_ADC1_TRG2) # define ADC1_JEXTSEL_VALUE ADC1_JEXTSEL_HRTTRG2 -#elif defined(CONFIG_STM32_HRTIM_ADC1_TRG3) +#elif defined(CONFIG_STM32_HRTIM_ADC1_TRG4) # define ADC1_JEXTSEL_VALUE ADC1_JEXTSEL_HRTTRG4 #else # define ADC1_JEXTSEL_VALUE 0 @@ -1590,7 +1590,7 @@ #if defined(CONFIG_STM32_HRTIM_ADC2_TRG2) # define ADC2_JEXTSEL_VALUE ADC2_JEXTSEL_HRTTRG2 -#elif defined(CONFIG_STM32_HRTIM_ADC2_TRG3) +#elif defined(CONFIG_STM32_HRTIM_ADC2_TRG4) # define ADC2_JEXTSEL_VALUE ADC2_JEXTSEL_HRTTRG4 #else # define ADC2_JEXTSEL_VALUE 0 diff --git a/arch/arm/src/stm32/stm32_dac.c b/arch/arm/src/stm32/stm32_dac.c index f0027cf09f..1a1d2f6226 100644 --- a/arch/arm/src/stm32/stm32_dac.c +++ b/arch/arm/src/stm32/stm32_dac.c @@ -1007,7 +1007,20 @@ static int dac_send(FAR struct dac_dev_s *dev, FAR struct dac_msg_s *msg) /* Enable DAC Channel */ - stm32_dac_modify_cr(chan, 0, DAC_CR_EN); +#if defined(CONFIG_STM32_STM32F33XX) + /* For STM32F33XX we have to set BOFF/OUTEN bit for DAC1CH2 and DAC2CH1 + * REVISIT: what if we connect DAC internally with comparator ? + */ + + if (chan->intf > 0) + { + stm32_dac_modify_cr(chan, 0, DAC_CR_EN|DAC_CR_BOFF); + } + else +#endif + { + stm32_dac_modify_cr(chan, 0, DAC_CR_EN); + } #ifdef HAVE_DMA if (chan->hasdma) diff --git a/arch/arm/src/stm32/stm32_hrtim.c b/arch/arm/src/stm32/stm32_hrtim.c index 70f922a91e..6aaf38e739 100644 --- a/arch/arm/src/stm32/stm32_hrtim.c +++ b/arch/arm/src/stm32/stm32_hrtim.c @@ -254,7 +254,7 @@ # define HRTIM_TIME_MODE 0 #endif -#if defined(CONFIG_STM32_HRTIM_TIMA) && !defined( HRTIM_TIMA_UPDATE) +#if defined(CONFIG_STM32_HRTIM_TIMA) && !defined(HRTIM_TIMA_UPDATE) # warning "HRTIM_TIMA_UPDATE is not set. Set the default value 0" # define HRTIM_TIMA_UPDATE 0 #endif diff --git a/arch/arm/src/stm32/stm32_hrtim.h b/arch/arm/src/stm32/stm32_hrtim.h index 987423342c..bb5551455c 100644 --- a/arch/arm/src/stm32/stm32_hrtim.h +++ b/arch/arm/src/stm32/stm32_hrtim.h @@ -705,6 +705,7 @@ enum stm32_hrtim_chopper_freq_e enum stm32_hrtim_adc_trq13_e { + HRTIM_ADCTRG13_NONE = 0, /* No trigger */ HRTIM_ADCTRG13_MC1 = (1 << 0), /* Trigger on Master Compare 1 */ HRTIM_ADCTRG13_MC2 = (1 << 1), /* Trigger on Master Compare 2 */ HRTIM_ADCTRG13_MC3 = (1 << 2), /* Trigger on Master Compare 3 */ @@ -749,6 +750,7 @@ enum stm32_hrtim_adc_trq13_e enum stm32_hrtim_adc_trq24_e { + HRTIM_ADCTRG24_NONE = 0, /* No trigger */ HRTIM_ADCTRG24_MC1 = (1 << 0), /* Trigger on Master Compare 1 */ HRTIM_ADCTRG24_MC2 = (1 << 1), /* Trigger on Master Compare 2 */ HRTIM_ADCTRG24_MC3 = (1 << 2), /* Trigger on Master Compare 3 */ diff --git a/configs/sim/dsptest/defconfig b/configs/sim/dsptest/defconfig new file mode 100644 index 0000000000..952570ed90 --- /dev/null +++ b/configs/sim/dsptest/defconfig @@ -0,0 +1,33 @@ +CONFIG_ARCH="sim" +CONFIG_ARCH_BOARD="sim" +CONFIG_ARCH_BOARD_SIM=y +CONFIG_ARCH_SIM=y +CONFIG_BUILTIN=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_ASSERTIONS=y +CONFIG_DISABLE_POLL=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EXAMPLES_DSPTEST=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INPUT=y +CONFIG_LIBDSP=y +CONFIG_LIBDSP_DEBUG=y +CONFIG_LIBDSP_PRECISION=2 +CONFIG_LIBM=y +CONFIG_LIB_BOARDCTL=y +CONFIG_MAX_TASKS=64 +CONFIG_MQ_MAXMSGSIZE=64 +CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_PTHREAD_STACK_DEFAULT=8192 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_START_DAY=6 +CONFIG_START_MONTH=10 +CONFIG_START_YEAR=2011 +CONFIG_SYSTEM_READLINE=y +CONFIG_TESTING_UNITY=y +CONFIG_TESTING_UNITY_OUTPUT_COLOR=y +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_USER_ENTRYPOINT="dsptest_main" diff --git a/drivers/power/motor.c b/drivers/power/motor.c index d050b85468..76f6e7546a 100644 --- a/drivers/power/motor.c +++ b/drivers/power/motor.c @@ -427,6 +427,20 @@ static int motor_ioctl(FAR struct file *filep, int cmd, unsigned long arg) goto errout; } +#ifdef CONFIG_MOTOR_HAVE_DIRECTION + /* Check direction configuration */ + + if (params->direction != MOTOR_DIR_CCW && + params->direction != MOTOR_DIR_CW) + { + pwrerr("ERROR: invalid direction value %d\n", + params->direction); + + ret = -EPERM; + goto errout; + } +#endif + #ifdef CONFIG_MOTOR_HAVE_POSITION /* Check position configuration */ diff --git a/include/dsp.h b/include/dsp.h index e455429222..bcc892369a 100644 --- a/include/dsp.h +++ b/include/dsp.h @@ -41,40 +41,107 @@ ****************************************************************************/ #include +#include #include +#include +#include #include +#include + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ +#ifndef CONFIG_BUILD_FLAT +# error "Only flat build supported for now" +#endif + +/* 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 +#else +# undef DEBUGASSERT +# define DEBUGASSERT(x) +#endif + +#ifndef CONFIG_LIBDSP_PRECISION +# define CONFIG_LIBDSP_PRECISION 0 +#endif + +/* Phase rotation direction */ + +#define DIR_CW (1.0f) +#define DIR_CCW (-1.0f) + /* Some math constants *********************************************************/ -#define SQRT3_BY_TWO_F ((float)0.866025) -#define SQRT3_BY_THREE_F ((float)0.57735) -#define ONE_BY_SQRT3_F ((float)0.57735) -#define TWO_BY_SQRT3_F ((float)1.15470) +#define SQRT3_BY_TWO_F (0.866025f) +#define SQRT3_BY_THREE_F (0.57735f) +#define ONE_BY_SQRT3_F (0.57735f) +#define TWO_BY_SQRT3_F (1.15470f) + +/* Some lib constants **********************************************************/ + +/* Motor electrical angle is in range 0.0 to 2*PI */ + +#define MOTOR_ANGLE_E_MAX (2.0f*M_PI_F) +#define MOTOR_ANGLE_E_MIN (0.0f) +#define MOTOR_ANGLE_E_RANGE (MOTOR_ANGLE_E_MAX - MOTOR_ANGLE_E_MIN) + +/* Motor mechanical angle is in range 0.0 to 2*PI */ + +#define MOTOR_ANGLE_M_MAX (2.0f*M_PI_F) +#define MOTOR_ANGLE_M_MIN (0.0f) +#define MOTOR_ANGLE_M_RANGE (MOTOR_ANGLE_M_MAX - MOTOR_ANGLE_M_MIN) /* Some useful macros ***************************************************************/ -/* Simple single-pole digital low pass filter: - * Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n))) +/**************************************************************************** + * Name: LP_FILTER * - * filter - (0.0 - 1.0) where 1.0 gives unfiltered values - * filter = T * (2*PI) * f_c + * Description: + * Simple single-pole digital low pass filter: + * Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n))) * - * phase shift = -arctan(f_in/f_c) + * filter - (0.0 - 1.0) where 1.0 gives unfiltered values + * filter = T * (2*PI) * 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 + * 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(val, sample, filter) val -= (filter * (val - sample)) +/**************************************************************************** + * Name: SVM3_BASE_VOLTAGE_GET + * + * 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(vbus) (vbus * SQRT3_BY_THREE_F) + /**************************************************************************** * Public Types ****************************************************************************/ @@ -92,6 +159,28 @@ struct phase_angle_s typedef struct phase_angle_s phase_angle_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_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 */ +}; + /* Float number saturaton */ struct float_sat_s @@ -160,6 +249,17 @@ struct svm3_state_s float d_u; /* Duty cycle for phase U */ float d_v; /* Duty cycle for phase V */ float d_w; /* Duty cycle for phase W */ + float d_max; /* Duty cycle max */ + float d_min; /* Duty cycle min */ +}; + +/* Motor open-loop control data */ + +struct openloop_data_s +{ + float max; /* Open-loop max speed */ + float angle; /* Open-loop current angle normalized to <0.0, 2PI> */ + float per; /* Open-loop control execution period */ }; /* Common motor observer structure */ @@ -170,6 +270,11 @@ struct motor_observer_s float speed; /* Estimated observer speed */ float per; /* Observer execution period */ + float angle_err; /* Observer angle error. + * This can be used to gradually eliminate + * error between openloop angle and observer angle + */ + /* There are different types of motor observers which different * sets of private data. */ @@ -178,6 +283,27 @@ struct motor_observer_s void *ao; /* Angle estimation observer data */ }; +/* Speed observer division method data */ + +struct motor_sobserver_div_s +{ + float angle_diff; /* Mechanical angle difference */ + float angle_acc; /* Accumulated mechanical angle */ + float angle_prev; /* Previous mechanical angle */ + float one_by_dt; /* Frequency of observer execution */ + float cntr; /* Sample counter */ + float samples; /* Number of samples for observer */ + float filter; /* Low-pass filter for final omega */ +}; + +/* Speed observer PLL method data */ +#if 0 +struct motor_sobserver_pll_s +{ + /* TODO */ +}; +#endif + /* Motor Sliding Mode Observer private data */ struct motor_observer_smo_s @@ -189,6 +315,7 @@ struct motor_observer_smo_s 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 */ @@ -203,13 +330,42 @@ struct motor_observer_smo_s struct motor_phy_params_s { - uint8_t poles; /* Number of the motor poles */ - float res; /* Phase-to-neutral temperature compensated resistance */ - float ind; /* Average phase-to-neutral inductance */ + uint8_t p; /* Number of the motor pole pairs */ + float res; /* Phase-to-neutral temperature compensated + * resistance + */ float res_base; /* Phase-to-neutral base resistance */ float res_alpha; /* Temperature coefficient of resistance */ float res_temp_ref; /* Reference temperature of alpha */ + float ind; /* Average phase-to-neutral inductance */ + float one_by_ind; /* Inverse phase-to-neutral inductance */ +}; + +/* Field oriented control (FOC) data + * REVISIT: + */ + +struct foc_data_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 + * 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 */ + + dq_frame_t i_dq_ref; /* Currrent 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 */ + + float vdq_mag_max; /* Maximum dq voltage magnitude */ + float vab_mod_scale; /* Voltage alpha-beta modulation scale */ }; /**************************************************************************** @@ -223,16 +379,25 @@ float fast_sin2(float angle); float fast_cos(float angle); float fast_cos2(float angle); float fast_atan2(float y, float x); + 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); /* PID controller functions */ -void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI, float KD); -void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI); +void pid_controller_init(FAR pid_controller_t *pid, + float KP, float KI, float KD); +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 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); @@ -244,34 +409,72 @@ 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); + +/* 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); /* 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, + int32_t *c0, int32_t *c1, int32_t *c2); /* Field Oriented control */ -void foc_current_control(FAR pid_controller_t *id_pid, - FAR pid_controller_t *iq_pid, - FAR dq_frame_t *idq_ref, - FAR dq_frame_t *idq, - FAR float_sat_t *sat, - FAR dq_frame_t *v_dq); +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_init(FAR struct foc_data_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); /* BLDC/PMSM motor observers */ void motor_observer_init(FAR struct motor_observer_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); void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo, float kslide, float err_max); -void motor_observer_smo(FAR struct motor_observer_s *observer, +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); + FAR struct motor_phy_params_s *phy, float dir); +void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so, + uint8_t samples, float filer, float per); +void motor_sobserver_div(FAR struct motor_observer_s *o, + float angle, float dir); + +/* Motor openloop control */ + +void motor_openloop_init(FAR struct openloop_data_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); + +/* 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, + float angle_new,float dir); +void motor_angle_m_update(FAR struct motor_angle_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); + +/* 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, + float res_alpha, float res_temp_ref); #undef EXTERN #if defined(__cplusplus) diff --git a/include/nuttx/power/motor.h b/include/nuttx/power/motor.h index 30b8f8e628..b64d6e517d 100644 --- a/include/nuttx/power/motor.h +++ b/include/nuttx/power/motor.h @@ -98,11 +98,20 @@ enum motor_fault_e { MOTOR_FAULT_OVERCURRENT = (1 << 0), /* Over-current Fault */ MOTOR_FAULT_OVERVOLTAGE = (1 << 1), /* Over-voltage Fault */ - MOTOR_FAULT_OVERPOWER = (1 << 2), /* Over-power Fault */ + MOTOR_FAULT_OVERPOWER = (1 << 2), /* Over-power Fault (electrical) */ MOTOR_FAULT_OVERTEMP = (1 << 3), /* Over-temperature Fault */ - MOTOR_FAULT_LOCKED = (1 << 4), /* Motor locked Fault */ - MOTOR_FAULT_INVAL_PARAM = (1 << 5), /* Invalid parameter Fault */ - MOTOR_FAULT_OTHER = (1 << 6) /* Other Fault */ + MOTOR_FAULT_OVERLOAD = (1 << 4), /* Motor overload Fault (mechanical) */ + MOTOR_FAULT_LOCKED = (1 << 5), /* Motor locked Fault */ + MOTOR_FAULT_INVAL_PARAM = (1 << 6), /* Invalid parameter Fault */ + MOTOR_FAULT_OTHER = (1 << 7) /* Other Fault */ +}; + +/* Motor direction */ + +enum motor_direction_e +{ + MOTOR_DIR_CCW = -1, + MOTOR_DIR_CW = 1 }; /* This structure contains feedback data from motor driver */ @@ -191,7 +200,7 @@ struct motor_params_s * parameter during run-time. */ #ifdef CONFIG_MOTOR_HAVE_DIRECTION - bool direction; /* Motor movement direction. We do not + int8_t direction; /* Motor movement direction. We do not * support negative values for parameters, * so this flag can be used to allow movement * in the positive and negative direction in @@ -214,7 +223,7 @@ struct motor_params_s float acceleration; /* Motor acceleration */ #endif #ifdef CONFIG_MOTOR_HAVE_DECELERATION - float deceleartion; /* Motor deceleration */ + float deceleration; /* Motor deceleration */ #endif }; diff --git a/libs/libdsp/Kconfig b/libs/libdsp/Kconfig index f14ff5fa44..9cc9117729 100644 --- a/libs/libdsp/Kconfig +++ b/libs/libdsp/Kconfig @@ -8,3 +8,23 @@ config LIBDSP default n ---help--- Enable build for various DSP functions + +config LIBDSP_DEBUG + bool "Libdsp debugging" + depends on LIBDSP + default n + ---help--- + Enable debugging for libdsp. This option enables additional parameters + checking. It can drastically reduce performance and be potentially + dangerous to hardware, so it should be used carefully (probably only + at an early stage of application development). + +config LIBDSP_PRECISION + int "Libdsp precision [0/1/2]" + default 0 + ---help--- + Whith this option we can select libdsp precision for + some of calculations. There are 3 available options: + 0 - the fastest calculation but the lowest precision + 1 - a little better precision than above, but slowest + 2 - the most accuracte but the slowest one, use standard math functions. diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile index c98ee9b752..0fa77eb903 100644 --- a/libs/libdsp/Makefile +++ b/libs/libdsp/Makefile @@ -49,6 +49,7 @@ CSRCS += lib_transform.c CSRCS += lib_observer.c CSRCS += lib_foc.c CSRCS += lib_misc.c +CSRCS += lib_motor.c endif AOBJS = $(ASRCS:.S=$(OBJEXT)) diff --git a/libs/libdsp/lib_foc.c b/libs/libdsp/lib_foc.c index 75b84dea80..ff8d9bdb56 100644 --- a/libs/libdsp/lib_foc.c +++ b/libs/libdsp/lib_foc.c @@ -37,6 +37,9 @@ * Included Files ****************************************************************************/ +#include +#include + #include /**************************************************************************** @@ -47,10 +50,6 @@ * Private Functions ****************************************************************************/ -/**************************************************************************** - * Public Functions - ****************************************************************************/ - /**************************************************************************** * Name: foc_current_control * @@ -58,43 +57,228 @@ * This function implements FOC current control algorithm. * * Input Parameters: - * id_pid - (in) pointer to the direct current PI controller data - * iq_pid - (in) pointer to the quadrature current PI controller data - * idq_ref - (in) dq current reference - * i_dq - (in) dq current - * vdq_min - (in) lower regulator limit - * vdq_max - (in) upper regulator limit - * v_dq - (out) pointer to the dq voltage frame + * foc - (in/out) pointer to the FOC data * * Returned Value: * None * ****************************************************************************/ -void foc_current_control(FAR pid_controller_t *id_pid, - FAR pid_controller_t *iq_pid, - FAR dq_frame_t *idq_ref, - FAR dq_frame_t *idq, - FAR float_sat_t *sat, - FAR dq_frame_t *v_dq) +static void foc_current_control(FAR struct foc_data_s *foc) { - dq_frame_t idq_err; + 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; /* Get dq current error */ - idq_err.d = idq_ref->d - idq->d; - idq_err.q = idq_ref->q - idq->q; + 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; - /* Update regulators saturation */ - - pi_saturation_set(id_pid, sat->min, sat->max); - pi_saturation_set(iq_pid, sat->min, sat->max); + /* NOTE: PI controllers saturation is updated in foc_vdq_mag_max_set() */ /* PI controller for d-current (flux loop) */ - v_dq->d = pi_controller(id_pid, idq_err.d); + v_dq->d = pi_controller(id_pid, foc->i_dq_err.d); /* PI controller for q-current (torque loop) */ - v_dq->q = pi_controller(iq_pid, idq_err.q); + v_dq->q = pi_controller(iq_pid, foc->i_dq_err.q); + + /* Saturate voltage DQ vector. + * The maximum DQ voltage magnitude depends on the maximum possible + * phase voltage and the maximum supported duty cycle. + */ + + dq_saturate(v_dq, foc->vdq_mag_max); +} + +/**************************************************************************** + * Name: foc_vab_mod_scale_set + * + * 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(FAR struct foc_data_s *foc, float scale) +{ + foc->vab_mod_scale = scale; +} + +/**************************************************************************** + * Name: foc_vdq_mag_max_set + * + * 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(FAR struct foc_data_s *foc, float max) +{ + foc->vdq_mag_max = max; + + /* Update regulators saturation */ + + pi_saturation_set(&foc->id_pid, -foc->vdq_mag_max, foc->vdq_mag_max); + pi_saturation_set(&foc->iq_pid, -foc->vdq_mag_max, foc->vdq_mag_max); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_init + * + * Description: + * Initialize FOC controller + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * id_kp - (in) KP for d current + * id_ki - (in) KI for d current + * iq_kp - (in) KP for q current + * iq_ki - (in) KI for q current + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_init(FAR struct foc_data_s *foc, + float id_kp, float id_ki, float iq_kp, float iq_ki) +{ + /* Reset data */ + + memset(foc, 0, sizeof(struct foc_data_s)); + + /* Initialize PI current d component */ + + pi_controller_init(&foc->id_pid, id_kp, id_ki); + + /* Initialize PI current q component */ + + pi_controller_init(&foc->iq_pid, iq_kp, iq_ki); +} + +/**************************************************************************** + * Name: foc_idq_ref_set + * + * Description: + * Set dq reference current vector + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * d - (in) reference d current + * q - (in) reference q current + * + * Returned Value: + * None + * + ****************************************************************************/ + +void foc_idq_ref_set(FAR struct foc_data_s *foc, float d, float q) +{ + foc->i_dq_ref.d = d; + foc->i_dq_ref.q = q; +} + +/**************************************************************************** + * Name: foc_vbase_update + * + * 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(FAR struct foc_data_s *foc, float vbase) +{ + float scale = 0.0f; + float mag_max = 0.0f; + + /* Only if voltage is valid */ + + if (vbase >= 0.0f) + { + scale = 1.0f/vbase; + mag_max = vbase; + } + + foc_vab_mod_scale_set(foc, scale); + foc_vdq_mag_max_set(foc, mag_max); +} + +/**************************************************************************** + * Name: foc_process + * + * Description: + * Process FOC (Field Oriented Control) + * + * Input Parameters: + * foc - (in/out) pointer to the FOC data + * i_abc - (in) pointer to the ABC current frame + * angle - (in) pointer to the phase angle data + * + * Returned Value: + * None + * + * TODO: add some reference and a brief description of the FOC + * + ****************************************************************************/ + +void foc_process(FAR struct foc_data_s *foc, + FAR abc_frame_t *i_abc, + FAR phase_angle_t *angle) +{ + DEBUGASSERT(foc != NULL); + DEBUGASSERT(i_abc != NULL); + DEBUGASSERT(angle != NULL); + + /* 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(&foc->i_abc, &foc->i_ab); + + /* Convert alpha-beta current to dq current */ + + park_transform(angle, &foc->i_ab, &foc->i_dq); + + /* Run FOC current control (current dq -> voltage dq) */ + + foc_current_control(foc); + + /* Inverse Park tranform (voltage dq -> voltage alpha-beta) */ + + inv_park_transform(angle, &foc->v_dq, &foc->v_ab); + + /* Normalize the alpha-beta voltage to get the alpha-beta modulation voltage */ + + foc->v_ab_mod.a = foc->v_ab.a * foc->vab_mod_scale; + foc->v_ab_mod.b = foc->v_ab.b * foc->vab_mod_scale; } diff --git a/libs/libdsp/lib_misc.c b/libs/libdsp/lib_misc.c index 4a50b7df8d..3ac815547e 100644 --- a/libs/libdsp/lib_misc.c +++ b/libs/libdsp/lib_misc.c @@ -80,6 +80,26 @@ void f_saturate(FAR float *val, float min, float max) } } +/**************************************************************************** + * Name: vector2d_mag + * + * Description: + * Get 2D vector magnitude. + * + * Input Parameters: + * x - (in) vector x component + * y - (in) vector y component + * + * Returned Value: + * Return 2D vector magnitude + * + ****************************************************************************/ + +float vector2d_mag(float x, float y) +{ + return sqrtf(x * x + y * y); +} + /**************************************************************************** * Name: vector2d_saturate * @@ -98,16 +118,16 @@ void f_saturate(FAR float *val, float min, float max) void vector2d_saturate(FAR float *x, FAR float *y, float max) { - float mag = 0.0; - float tmp = 0.0; + float mag = 0.0f; + float tmp = 0.0f; /* Get vector magnitude */ - mag = sqrtf(*x * *x + *y * *y); + mag = vector2d_mag(*x, *y); - if (mag < (float)1e-10) + if (mag < 1e-10f) { - mag = (float)1e-10; + mag = 1e-10f; } if (mag > max) @@ -120,6 +140,25 @@ void vector2d_saturate(FAR float *x, FAR float *y, float max) } } +/**************************************************************************** + * Name: dq_mag + * + * Description: + * Get DQ vector magnitude. + * + * Input Parameters: + * dq - (in/out) dq frame vector + * + * Returned Value: + * Return dq vector magnitude + * + ****************************************************************************/ + +float dq_mag(FAR dq_frame_t *dq) +{ + return vector2d_mag(dq->d, dq->q); +} + /**************************************************************************** * Name: dq_saturate * @@ -158,9 +197,9 @@ void dq_saturate(FAR dq_frame_t *dq, float max) float fast_sin(float angle) { - float sin = 0.0; - float n1 = 1.27323954; - float n2 = 0.405284735; + float sin = 0.0f; + float n1 = 1.27323954f; + float n2 = 0.405284735f; /* Normalize angle */ @@ -168,7 +207,7 @@ float fast_sin(float angle) /* Get estiamte sine value from quadratic equation */ - if (angle < 0.0) + if (angle < 0.0f) { sin = n1 * angle + n2 * angle * angle; } @@ -220,10 +259,10 @@ float fast_cos(float angle) float fast_sin2(float angle) { - float sin = 0.0; - float n1 = 1.27323954; - float n2 = 0.405284735; - float n3 = 0.225; + float sin = 0.0f; + float n1 = 1.27323954f; + float n2 = 0.405284735f; + float n3 = 0.225f; /* Normalize angle */ @@ -231,11 +270,11 @@ float fast_sin2(float angle) /* Get estiamte sine value from quadratic equation and do more */ - if (angle < 0.0) + if (angle < 0.0f) { sin = n1 * angle + n2 * angle * angle; - if (sin < 0) + if (sin < 0.0f) { sin = n3 * (sin *(-sin) - sin) + sin; } @@ -248,7 +287,7 @@ float fast_sin2(float angle) { sin = n1 * angle - n2 * angle * angle; - if (sin < 0) + if (sin < 0.0f) { sin = n3 * (sin *(-sin) - sin) + sin; } @@ -302,35 +341,35 @@ float fast_cos2(float angle) float fast_atan2(float y, float x) { - float angle = 0.0; - float abs_y = 0.0; - float rsq = 0.0; - float r = 0.0; - float n1 = 0.1963; - float n2 = 0.9817; + float angle = 0.0f; + float abs_y = 0.0f; + float rsq = 0.0f; + float r = 0.0f; + float n1 = 0.1963f; + float n2 = 0.9817f; /* Get absolute value of y and add some small number to prevent 0/0 */ - abs_y = fabsf(y)+(float)1e-10; + abs_y = fabsf(y)+1e-10f; /* Calculate angle */ - if (x >= 0.0) + if (x >= 0.0f) { r = (x - abs_y) / (x + abs_y); rsq = r * r; - angle = ((n1 * rsq) - n2) * r + (float)(M_PI_F / 4.0); + angle = ((n1 * rsq) - n2) * r + (M_PI_F / 4.0f); } else { r = (x + abs_y) / (abs_y - x); rsq = r * r; - angle = ((n1 * rsq) - n2) * r + (float)(3.0 * M_PI_F / 4.0); + angle = ((n1 * rsq) - n2) * r + (3.0f * M_PI_F / 4.0f); } /* Get angle sign */ - if (y < 0.0) + if (y < 0.0f) { angle = -angle; } @@ -394,7 +433,7 @@ void angle_norm(FAR float *angle, float per, float bottom, float top) void angle_norm_2pi(FAR float *angle, float bottom, float top) { - angle_norm(angle, 2*M_PI_F, bottom, top); + angle_norm(angle, 2.0f*M_PI_F, bottom, top); } /**************************************************************************** @@ -417,13 +456,24 @@ void angle_norm_2pi(FAR float *angle, float bottom, float top) void phase_angle_update(FAR struct phase_angle_s *angle, float val) { + DEBUGASSERT(angle != NULL); + /* Normalize angle to <0.0, 2PI> */ - angle_norm_2pi(&val, 0.0, 2*M_PI_F); + angle_norm_2pi(&val, 0.0f, 2.0f*M_PI_F); /* Update structure */ angle->angle = val; + +#if CONFIG_LIBDSP_PRECISION == 1 + angle->sin = fast_sin2(val); + angle->cos = fast_cos2(val); +#elif CONFIG_LIBDSP_PRECISION == 2 + angle->sin = sin(val); + angle->cos = cos(val); +#else angle->sin = fast_sin(val); angle->cos = fast_cos(val); +#endif } diff --git a/libs/libdsp/lib_motor.c b/libs/libdsp/lib_motor.c new file mode 100644 index 0000000000..7998a1ec9e --- /dev/null +++ b/libs/libdsp/lib_motor.c @@ -0,0 +1,426 @@ +/**************************************************************************** + * control/lib_motor.c + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define POLE_CNTR_THR 0.0f + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: motor_openloop_init + * + * Description: + * Initialize open-loop data + * + * Input Parameters: + * op - (in/out) pointer to the openlooop data structure + * max - (in) maximum openloop speed + * per - (in) period of the open-loop control + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_openloop_init(FAR struct openloop_data_s *op, float max, float per) +{ + DEBUGASSERT(op != NULL); + DEBUGASSERT(max > 0.0f); + DEBUGASSERT(per > 0.0f); + + /* Reset openloop structure */ + + memset(op, 0, sizeof(struct openloop_data_s)); + + /* Initialize data */ + + op->max = max; + op->per = per; +} + +/**************************************************************************** + * Name: motor_openloop + * + * Description: + * One step of the open-loop control + * + * Input Parameters: + * op - (in/out) pointer to the open-loop data structure + * speed - (in) open-loop speed + * dir - (in) rotation direction + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir) +{ + DEBUGASSERT(op != NULL); + DEBUGASSERT(speed >= 0.0f); + DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); + + float phase_step = 0.0f; + + /* REVISIT: what should we do if speed is greater than max open-loop speed? + * Saturate speed or stop motor ? + */ + + if (speed > op->max) + { + speed = op->max; + } + + /* Get phase step */ + + phase_step = dir * speed * op->per; + + /* Update open-loop angle */ + + op->angle += phase_step; + + /* Normalize the open-looop angle to 0.0 - 2PI range */ + + angle_norm_2pi(&op->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX); +} + +/**************************************************************************** + * Name: motor_openloop_angle_get + * + * Description: + * Get angle from open-loop controller + * + * Input Parameters: + * op - (in/out) pointer to the open-loop data structure + * + * Returned Value: + * Return angle from open-loop controller + * + ****************************************************************************/ + +float motor_openloop_angle_get(FAR struct openloop_data_s *op) +{ + DEBUGASSERT(op != NULL); + + return op->angle; +} + +/* In a multipolar electrical machines, mechanical angle is not equal + * to mechanical angle which can be described as: + * + * electrical angle = (p) * mechanical angle + * where p - number of poles pair + * + * + * electrical + * angle: + * + * poles = 4 + * i = 0 1 2 3 0 1 2 3 0 + * 2PI ______________________________________________ + * /| /| /| /| /| /| /| /| /| + * / | / | / | / | / | / | / | / | / | + * / | / | / | / | / | / | / | / | / | + * 0 /___|/___|/___|/___|/___|/___|/___|/___|/___|/ + * . . . + * mechanical . . . + * angle: . . . + * . . . + * 2PI .__________________.___________________.______ + * . o o + * . o o o o + * . o o o o + * . o o o o o + * 0 o__________________o___________________o______ + * + */ + +/**************************************************************************** + * Name: motor_angle_init + * + * Description: + * Initialize motor angle structure + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle strucutre + * p - (in) number of the motor pole pairs + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p) +{ + DEBUGASSERT(angle != NULL); + DEBUGASSERT(p > 0); + + /* Reset structure */ + + memset(angle, 0, sizeof(struct motor_angle_s)); + + /* Store pole pairs */ + + angle->p = p; + angle->one_by_p = (float)1.0f/p; + + /* Initialize angle with 0.0 */ + + phase_angle_update(&angle->angle_el, 0.0f); +} + +/**************************************************************************** + * Name: motor_angle_e_update + * + * 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(FAR struct motor_angle_s *angle, float angle_new, float dir) +{ + DEBUGASSERT(angle != NULL); + DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX); + DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); + + /* Check if we crossed electrical angle boundaries */ + + if (dir == DIR_CW && + angle_new - angle->angle_el.angle < -POLE_CNTR_THR) + { + /* For CW direction - previous angle is greater than current angle */ + + angle->i += 1; + } + + else if (dir == DIR_CCW && + angle_new - angle->angle_el.angle > POLE_CNTR_THR) + { + /* For CCW direction - previous angle is lower than current angle */ + + 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(&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. + */ + + angle->anglem = (MOTOR_ANGLE_E_RANGE * angle->i + + angle->angle_el.angle) * angle->one_by_p; + + /* Normalize mechanical angle to <0, 2PI> and store */ + + angle_norm_2pi(&angle->anglem, MOTOR_ANGLE_M_MIN, MOTOR_ANGLE_M_MAX); +} + +/**************************************************************************** + * Name: motor_angle_m_update + * + * 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(FAR struct motor_angle_s *angle, float angle_new, float dir) +{ + DEBUGASSERT(angle != NULL); + DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX); + DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW); + + float angle_el = 0.0f; + + /* Store new mechanical angle */ + + angle->anglem = angle_new; + + /* Update pole counter */ + + angle->i = (uint8_t)(angle->anglem * angle->p/MOTOR_ANGLE_M_MAX); + + /* Get electrical angle */ + + angle_el = angle->anglem * angle->p - MOTOR_ANGLE_E_MAX * angle->i; + + /* Update electrical angle structure */ + + phase_angle_update(&angle->angle_el, angle_el); +} + +/**************************************************************************** + * Name: motor_angle_m_get + * + * Description: + * Get motor mechanical angle + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * + * Returned Value: + * Return motor mechanical angle + * + ****************************************************************************/ + +float motor_angle_m_get(FAR struct motor_angle_s *angle) +{ + DEBUGASSERT(angle != NULL); + + return angle->anglem; +} + +/**************************************************************************** + * Name: motor_angle_e_get + * + * Description: + * Get motor electrical angle + * + * Input Parameters: + * angle - (in/out) pointer to the motor angle structure + * + * Returned Value: + * Return motor electrical angle + * + ****************************************************************************/ + +float motor_angle_e_get(FAR struct motor_angle_s *angle) +{ + DEBUGASSERT(angle != NULL); + + return angle->angle_el.angle; +} + +/**************************************************************************** + * Name: motor_phy_params_init + * + * 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(FAR struct motor_phy_params_s *phy, uint8_t poles, + float res, float ind) +{ + DEBUGASSERT(phy != NULL); + + phy->p = poles; + phy->res_base = res_base; + phy->ind = ind; + phy->one_by_ind = 1.0f/ind; + + /* Initialize with zeros */ + + phy->res = 0.0f; + phy->res_alpha = 0.0f; + phy->res_temp_ref = 0.0f; +} + +/**************************************************************************** + * Name: motor_phy_params_temp_set + * Description: + * Initialize motor physical temperature parameters + * + * Input Parameters: + * phy - (in/out) pointer to the motor physical parameters + * res_alpha - (in) temperature coefficient of the winding resistance + * res_temp_ref - (in) reference temperature for alpha + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy, + float res_alpha, float res_temp_ref) +{ + DEBUGASSERT(phy != NULL); + + phy->res_alpha = res_alpha; + phy->res_temp_ref = res_temp_ref; +} diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c index 44d671dd95..a94af4d95a 100644 --- a/libs/libdsp/lib_observer.c +++ b/libs/libdsp/lib_observer.c @@ -37,15 +37,14 @@ * Included Files ****************************************************************************/ -#include -#include - #include /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ +#define ANGLE_DIFF_THR M_PI_F + /**************************************************************************** * Private Functions ****************************************************************************/ @@ -77,7 +76,11 @@ void motor_observer_init(FAR struct motor_observer_s *observer, DEBUGASSERT(observer != NULL); DEBUGASSERT(ao != NULL); DEBUGASSERT(so != NULL); - DEBUGASSERT(per > 0.0); + DEBUGASSERT(per > 0.0f); + + /* Reset observer data */ + + memset(observer, 0, sizeof(struct motor_observer_s)); /* Set observer period */ @@ -113,8 +116,12 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo, float err_max) { DEBUGASSERT(smo != NULL); - DEBUGASSERT(kslide > 0.0); - DEBUGASSERT(err_max > 0.0); + DEBUGASSERT(kslide > 0.0f); + DEBUGASSERT(err_max > 0.0f); + + /* Reset structure */ + + memset(smo, 0, sizeof(struct motor_observer_smo_s)); /* Initialize structure */ @@ -168,50 +175,59 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo, * z - output correction factor voltage * * Input Parameters: - * observer - (in/out) pointer to the common observer data - * i_ab - (in) inverter alpha-beta current - * v_ab - (in) inverter alpha-beta voltage - * phy - (in) pointer to the motor physical parameters + * o - (in/out) pointer to the common observer data + * i_ab - (in) inverter alpha-beta current + * v_ab - (in) inverter alpha-beta voltage + * phy - (in) pointer to the motor physical parameters + * dir - (in) rotation direction (1.0 for CW, -1.0 for CCW) * * Returned Value: * None * ****************************************************************************/ -void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i_ab, - FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy) +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) { + 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 *)observer->ao; + (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; - float i_err_a_abs = 0.0; - float i_err_b_abs = 0.0; - float angle = 0.0; + float i_err_a_abs = 0.0f; + float i_err_b_abs = 0.0f; + float angle = 0.0f; + float filter = 0.0f; /* REVISIT: observer works only when IQ current is high enough */ /* Calculate observer gains */ - smo->F_gain = (1 - observer->per*phy->res/phy->ind); - smo->G_gain = observer->per/phy->ind; + smo->F_gain = (1.0f - o->per*phy->res*phy->one_by_ind); + smo->G_gain = o->per*phy->one_by_ind; /* Saturate F gain */ - if (smo->F_gain < 0) + if (smo->F_gain < 0.0f) { - smo->F_gain = 0.0; + smo->F_gain = 0.0f; } /* Saturate G gain */ - if (smo->G_gain > 0.999) + if (smo->G_gain > 0.999f) { - smo->G_gain = 0.999; + smo->G_gain = 0.999f; } /* Configure low pass filters @@ -220,27 +236,45 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i * input singal frequency. This gives us constant phase shift between * input and outpu signals equals to: * - * phi = -arctan(f_in/f_c) = -arctan(1) = -PI/2 + * phi = -arctan(f_in/f_c) = -arctan(1) = -45deg = -PI/4 * * Input signal frequency is equal to the frequency of the motor currents, * which give us: * * f_c = omega_e/(2*PI) - * omega_m = omega_e/poles - * f_c = omega_m*poles/(2*PI) + * omega_m = omega_e/pole_pairs + * f_c = omega_m*pole_pairs/(2*PI) * * filter = T * (2*PI) * f_c - * filter = T * omega_m * poles + * filter = T * omega_m * pole_pairs * - * T - [s] period at which the digital filter is being calculated - * f_in - [Hz] input frequency of the filter - * f_c - [Hz] cutoff frequency of the filter - * omega_m - [rad/s] mechanical angular velocity - * omega_e - [rad/s] electrical angular velocity + * T - [s] period at which the digital filter is being calculated + * f_in - [Hz] input frequency of the filter + * f_c - [Hz] cutoff frequency of the filter + * omega_m - [rad/s] mechanical angular velocity + * omega_e - [rad/s] electrical angular velocity + * pole_pairs - pole pairs * */ - smo->emf_lp_filter1 = observer->per * observer->speed * phy->poles; + filter = o->per * o->speed * phy->p; + + /* Limit SMO filters + * REVISIT: lowest filter limit should depend on minimum speed: + * filter = T * (2*PI) * f_c = T * omega0 + * + */ + + if (filter >= 1.0f) + { + filter = 0.99f; + } + else if (filter <= 0.0f) + { + filter = 0.005f; + } + + smo->emf_lp_filter1 = filter; smo->emf_lp_filter2 = smo->emf_lp_filter1; /* Get voltage error: v_err = v_ab - emf */ @@ -260,8 +294,8 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i /* Slide-mode controller */ - sign->a = (i_err->a > 0 ? 1.0 : -1.0); - sign->b = (i_err->b > 0 ? 1.0 : -1.0); + sign->a = (i_err->a > 0.0f ? 1.0f : -1.0f); + sign->b = (i_err->b > 0.0f ? 1.0f : -1.0f); /* Get current error absolute value - just multiply value with its sign */ @@ -303,8 +337,8 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i /* Filter emf one more time before angle stimation */ - LP_FILTER(emf->a, emf->a, smo->emf_lp_filter2); - LP_FILTER(emf->b, emf->b, smo->emf_lp_filter2); + LP_FILTER(emf_f->a, emf->a, smo->emf_lp_filter2); + LP_FILTER(emf_f->b, emf->b, smo->emf_lp_filter2); /* Estimate phase angle according to: * emf_a = -|emf| * sin(th) @@ -319,29 +353,211 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i * TODO: simplify */ - if (angle != angle) angle = 0.0; - if (emf->a != emf->a) emf->a = 0.0; - if (emf->b != emf->b) emf->b = 0.0; - if (z->a != z->a) z->a = 0.0; - if (z->b != z->b) z->b = 0.0; - if (i_est->a != i_est->a) i_est->a = 0.0; - if (i_est->b != i_est->b) i_est->b = 0.0; + if (angle != angle) angle = 0.0f; + if (emf->a != emf->a) emf->a = 0.0f; + if (emf->b != emf->b) emf->b = 0.0f; + if (z->a != z->a) z->a = 0.0f; + if (z->b != z->b) z->b = 0.0f; + if (i_est->a != i_est->a) i_est->a = 0.0f; + if (i_est->b != i_est->b) i_est->b = 0.0f; #endif /* Angle compensation. * Due to low pass filtering we have some delay in estimated phase angle. * - * Adaptive filters introduced above cause -PI/2 phase shift for each filter. - * We use 2 times filtering which give us constant -PI phase shift. + * Adaptive filters introduced above cause -PI/4 phase shift for each filter. + * We use 2 times filtering which give us constant -PI/2 (-90deg) phase shift. */ - angle = angle -M_PI_F; + angle = angle + dir * M_PI_2_F; /* Normalize angle to range <0, 2PI> */ - angle_norm_2pi(&angle, 0.0, 2*M_PI_F); + angle_norm_2pi(&angle, 0.0f, 2.0f*M_PI_F); /* Store estimated angle in observer data*/ - observer->angle = angle; + o->angle = angle; +} + +/**************************************************************************** + * Name: motor_sobserver_div_init + * + * Description: + * Initialize DIV speed observer + * + * Input Parameters: + * so - (in/out) pointer to the DIV speed observer data + * sample - (in) number of mechanical angle samples + * filter - (in) low-pass filter for final omega + * per - (in) speed observer execution period + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so, + uint8_t samples, + float filter, + float per) +{ + DEBUGASSERT(so != NULL); + DEBUGASSERT(samples > 0); + DEBUGASSERT(filter > 0.0f); + + /* Reset observer data */ + + memset(so, 0, sizeof(struct motor_sobserver_div_s)); + + /* Store number of samples for DIV observer */ + + so->samples = samples; + + /* Store low-pass filter for DIV observer speed */ + + so->filter = filter; + + /* */ + + so->one_by_dt = 1.0f/(so->samples * per); +} + + +/**************************************************************************** + * Name: motor_sobserver_div + * + * Description: + * Estimate motor mechanical speed based on motor mechanical angle + * difference. + * + * Input Parameters: + * o - (in/out) pointer to the common observer data + * angle - (in) mechanical angle normalized to <0.0, 2PI> + * dir - (in) mechanical rotation direction. Valid values: + * DIR_CW (1.0f) or DIR_CCW(-1.0f) + * + ****************************************************************************/ + +void motor_sobserver_div(FAR struct motor_observer_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; + volatile float omega = 0.0f; + + /* Get angle diff */ + + so->angle_diff = angle - so->angle_prev; + + /* Correct angle if we crossed angle boundary + * REVISIT: + */ + + if ((dir == DIR_CW && so->angle_diff < -ANGLE_DIFF_THR) || + (dir == DIR_CCW && so->angle_diff > ANGLE_DIFF_THR)) + { + /* Correction sign depends on rotation direction */ + + so->angle_diff += dir*2*M_PI_F; + } + + /* Get absoulte value */ + + if (so->angle_diff < 0.0f) + { + so->angle_diff = -so->angle_diff; + } + + /* Accumulate angle only if sample is valid */ + + so->angle_acc += so->angle_diff; + + /* Increase counter */ + + so->cntr += 1; + + /* Accumulate angle until we get configured number of samples */ + + if (so->cntr >= so->samples) + { + /* Estimate omega using accumulated angle samples. + * In this case use simple estimation: + * + * omega = delta_theta/delta_time + * speed_now = low_pass(omega) + * + */ + + omega = so->angle_acc*so->one_by_dt; + + /* Store filtered omega. + * + * REVISIT: cut-off frequency for this filter should be + * (probably) set according to minimum supported omega: + * + * filter = T * (2*PI) * f_c = T * omega0 + * + * where: + * omega0 - minimum angular speed + * T - speed estimation period (samples*one_by_dt) + */ + + LP_FILTER(o->speed, omega, so->filter); + + /* Reset samples counter and accumulated angle */ + + so->cntr = 0; + so->angle_acc = 0.0f; + } + + /* Store current angle as previous angle */ + + so->angle_prev = angle; +} + +/**************************************************************************** + * Name: motor_observer_speed_get + * + * Description: + * Get the estmiated motor mechanical speed from the observer + * + * Input Parameters: + * o - (in/out) pointer to the common observer data + * + * Returned Value: + * Return estimated motor mechanical speed from observer + * + ****************************************************************************/ + +float motor_observer_speed_get(FAR struct motor_observer_s *o) +{ + DEBUGASSERT(o != NULL); + + return o->speed; +} + +/**************************************************************************** + * Name: motor_observer_angle_get + * + * Description: + * Get the estmiated motor electrical angle from the observer + * + * Input Parameters: + * o - (in/out) pointer to the common observer data + * + * Returned Value: + * Return estimated motor mechanical angle from observer + * + ****************************************************************************/ + +float motor_observer_angle_get(FAR struct motor_observer_s *o) +{ + DEBUGASSERT(o != NULL); + + return o->angle; } diff --git a/libs/libdsp/lib_pid.c b/libs/libdsp/lib_pid.c index 466d2ce99f..ce248f2d36 100644 --- a/libs/libdsp/lib_pid.c +++ b/libs/libdsp/lib_pid.c @@ -37,8 +37,6 @@ * Included Files ****************************************************************************/ -#include - #include /**************************************************************************** @@ -70,6 +68,8 @@ void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI, float KD) { + DEBUGASSERT(pid != NULL); + /* Reset controller data */ memset(pid, 0, sizeof(pid_controller_t)); @@ -100,6 +100,8 @@ 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) { + DEBUGASSERT(pid != NULL); + /* Reset controller data */ memset(pid, 0, sizeof(pid_controller_t)); @@ -108,7 +110,7 @@ void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI) pid->KP = KP; pid->KI = KI; - pid->KD = 0.0; + pid->KD = 0.0f; } /**************************************************************************** @@ -129,8 +131,11 @@ 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_t *pid, float min, float max) { + DEBUGASSERT(pid != NULL); + DEBUGASSERT(min < max); + pid->sat.max = max; pid->sat.min = min; } @@ -150,11 +155,32 @@ 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_t *pid, float min, float max) { + DEBUGASSERT(pid != NULL); + DEBUGASSERT(min < max); + pid_saturation_set(pid, min, max); } +/**************************************************************************** + * Name: pid_integral_reset + ****************************************************************************/ + +void pid_integral_reset(FAR pid_controller_t *pid) +{ + pid->part[1] = 0.0f; +} + +/**************************************************************************** + * Name: pi_integral_reset + ****************************************************************************/ + +void pi_integral_reset(FAR pid_controller_t *pid) +{ + pid_integral_reset(pid); +} + /**************************************************************************** * Name: pi_controller * @@ -172,6 +198,8 @@ void pi_saturation_set(FAR pid_controller_t* pid, float min, float max) float pi_controller(FAR pid_controller_t *pid, float err) { + DEBUGASSERT(pid != NULL); + /* Store error in controller structure */ pid->err = err; @@ -188,9 +216,12 @@ float pi_controller(FAR pid_controller_t *pid, float err) pid->out = pid->part[0] + pid->part[1]; - /* Saturate output only if not in PID calculation and if some limits are set */ + /* Saturate output only if we are not in a PID calculation and only + * if some limits are set. Saturation for a PID controller are done later + * in PID routine. + */ - if (pid->sat.max != pid->sat.min && pid->KD == 0.0) + if (pid->sat.max != pid->sat.min && pid->KD == 0.0f) { if (pid->out > pid->sat.max) { @@ -200,9 +231,9 @@ float pi_controller(FAR pid_controller_t *pid, float err) /* Integral anti-windup - reset integral part */ - if (err > 0) + if (err > 0.0f) { - pid->part[1] = 0; + pi_integral_reset(pid); } } else if (pid->out < pid->sat.min) @@ -213,9 +244,9 @@ float pi_controller(FAR pid_controller_t *pid, float err) /* Integral anti-windup - reset integral part */ - if (err < 0) + if (err < 0.0f) { - pid->part[1] = 0; + pi_integral_reset(pid); } } } @@ -242,6 +273,8 @@ float pi_controller(FAR pid_controller_t *pid, float err) float pid_controller(FAR pid_controller_t *pid, float err) { + DEBUGASSERT(pid != NULL); + /* Get PI output */ pi_controller(pid, err); diff --git a/libs/libdsp/lib_svm.c b/libs/libdsp/lib_svm.c index 1b58aaede4..87fc419480 100644 --- a/libs/libdsp/lib_svm.c +++ b/libs/libdsp/lib_svm.c @@ -56,7 +56,7 @@ * Get current sector 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 @@ -146,7 +146,8 @@ static uint8_t svm3_sector_get(FAR abc_frame_t *ijk) * Calculate duty cycles for space vector modulation. * * Input Parameters: - * s - (in/out) pointer to the SVM state data + * s - (in/out) pointer to the SVM state data + * ijk - (in) pointer to the auxiliary ABC frame * * Returned Value: * None @@ -158,9 +159,9 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk) float i = ijk->a; float j = ijk->b; float k = ijk->c; - float T0 = 0.0; - float T1 = 0.0; - float T2 = 0.0; + float T0 = 0.0f; + float T1 = 0.0f; + float T2 = 0.0f; /* Determine T1, T2 and T0 based on the sector */ @@ -213,7 +214,7 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk) /* Get null vector time */ - T0 = (float)1.0 - T1 - T2; + T0 = 1.0f - T1 - T2; /* Calculate duty cycle for 3 phase */ @@ -221,44 +222,44 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk) { case 1: { - s->d_u = T1 + T2 + T0/2; - s->d_v = T2 + T0/2; - s->d_w = T0/2; + s->d_u = T1 + T2 + T0*0.5f; + s->d_v = T2 + T0*0.5f; + s->d_w = T0*0.5f; break; } case 2: { - s->d_u = T1 + T0/2; - s->d_v = T1 + T2 + T0/2; - s->d_w = T0/2; + s->d_u = T1 + T0*0.5f; + s->d_v = T1 + T2 + T0*0.5f; + s->d_w = T0*0.5f; break; } case 3: { - s->d_u = T0/2; - s->d_v = T1 + T2 + T0/2; - s->d_w = T2 + T0/2; + s->d_u = T0*0.5f; + s->d_v = T1 + T2 + T0*0.5f; + s->d_w = T2 + T0*0.5f; break; } case 4: { - s->d_u = T0/2; - s->d_v = T1 + T0/2; - s->d_w = T1 + T2 + T0/2; + s->d_u = T0*0.5f; + s->d_v = T1 + T0*0.5f; + s->d_w = T1 + T2 + T0*0.5f; break; } case 5: { - s->d_u = T2 + T0/2; - s->d_v = T0/2; - s->d_w = T1 + T2 + T0/2; + s->d_u = T2 + T0*0.5f; + s->d_v = T0*0.5f; + s->d_w = T1 + T2 + T0*0.5f; break; } case 6: { - s->d_u = T1 + T2 + T0/2; - s->d_v = T0/2; - s->d_w = T1 + T0/2; + s->d_u = T1 + T2 + T0*0.5f; + s->d_v = T0*0.5f; + s->d_w = T1 + T0*0.5f; break; } default: @@ -280,6 +281,7 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk) * * 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: * @@ -311,7 +313,13 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk) * Input Parameters: * s - (out) pointer to the SVM data * v_ab - (in) pointer to the modulation voltage vector in alpha-beta frame, - * normalized to <0.0 - 1.0> range + * 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. + * + * REVISIT: not sure how we should handle invalid data from user. + * For now we saturate output duty form SVM. * * REFERENCE: * https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download (32-34) @@ -320,15 +328,18 @@ 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) { + DEBUGASSERT(s != NULL); + DEBUGASSERT(v_ab != NULL); + abc_frame_t ijk; /* Perform modified inverse Clarke-transformation (alpha,beta) -> (i,j,k) - * to obtain auxliary frame which will be used in further calculations. + * to obtain auxiliary frame which will be used in further calculations. */ - ijk.a = -v_ab->b/2 + SQRT3_BY_TWO_F*v_ab->a; + ijk.a = -0.5f*v_ab->b + SQRT3_BY_TWO_F*v_ab->a; ijk.b = v_ab->b; - ijk.c = -v_ab->b/2 - SQRT3_BY_TWO_F*v_ab->a; + ijk.c = -ijk.b - ijk.a; /* Get vector sector */ @@ -337,4 +348,103 @@ void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab) /* Get duty cycle */ svm3_duty_calc(s, &ijk); + + /* Saturate output from SVM */ + + f_saturate(&s->d_u, s->d_min, s->d_max); + f_saturate(&s->d_v, s->d_min, s->d_max); + f_saturate(&s->d_w, s->d_min, s->d_max); +} + +/**************************************************************************** + * Name: svm3_current_correct + * + * Description: + * Correct ADC samples (int32) according to SVM3 state. + * NOTE: This works only with 3 shunt resistors configuration. + * + ****************************************************************************/ + +void svm3_current_correct(FAR struct svm3_state_s *s, + int32_t *c0, int32_t *c1, int32_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 + * + * Description: + * Initialize 3-phase SVM data. + * + * Input Parameters: + * s - (in/out) pointer to the SVM state data + * sat - (in) + * + * Returned Value: + * None + * + ****************************************************************************/ + +void svm3_init(FAR struct svm3_state_s *s, float min, float max) +{ + DEBUGASSERT(s != NULL); + DEBUGASSERT(max > min); + + memset(s, 0, sizeof(struct svm3_state_s)); + + s->d_max = max; + s->d_min = min; } diff --git a/libs/libdsp/lib_transform.c b/libs/libdsp/lib_transform.c index 2cb844d37f..16026fa715 100644 --- a/libs/libdsp/lib_transform.c +++ b/libs/libdsp/lib_transform.c @@ -72,6 +72,9 @@ void clarke_transform(FAR abc_frame_t *abc, FAR ab_frame_t *ab) { + DEBUGASSERT(abc != NULL); + DEBUGASSERT(ab != NULL); + ab->a = abc->a; ab->b = ONE_BY_SQRT3_F*abc->a + TWO_BY_SQRT3_F*abc->b; } @@ -94,10 +97,13 @@ void clarke_transform(FAR abc_frame_t *abc, void inv_clarke_transform(FAR ab_frame_t *ab, FAR abc_frame_t *abc) { + DEBUGASSERT(ab != NULL); + DEBUGASSERT(abc != NULL); + /* Assume non-power-invariant transform and balanced system */ abc->a = ab->a; - abc->b = -ab->a/2 + SQRT3_BY_TWO_F*ab->b; + abc->b = -0.5f*ab->a + SQRT3_BY_TWO_F*ab->b; abc->c = -abc->a - abc->b; } @@ -121,6 +127,10 @@ void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab, FAR dq_frame_t *dq) { + DEBUGASSERT(angle != NULL); + DEBUGASSERT(ab != NULL); + DEBUGASSERT(dq != NULL); + dq->d = angle->cos * ab->a + angle->sin * ab->b; dq->q = angle->cos * ab->b - angle->sin * ab->a; } @@ -145,6 +155,10 @@ void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq, FAR ab_frame_t *ab) { + DEBUGASSERT(angle != NULL); + DEBUGASSERT(dq != NULL); + DEBUGASSERT(ab != NULL); + ab->a = angle->cos * dq->d - angle->sin * dq->q; ab->b = angle->cos * dq->q + angle->sin * dq->d; }