From b5ec5349b00e0951bc641b77f5209ccffdcfaad4 Mon Sep 17 00:00:00 2001 From: Mateusz Szafoni Date: Wed, 30 May 2018 12:36:06 +0000 Subject: [PATCH] Merged in raiden00/nuttx (pull request #648) libdsp: initial commit * libdsp: initial commit * libdsp: cosmetics Approved-by: Gregory Nutt --- Kconfig | 4 + include/dsp.h | 285 ++++++++++++++++++++++++ libs/libdsp/Kconfig | 10 + libs/libdsp/Makefile | 88 ++++++++ libs/libdsp/README.txt | 6 + libs/libdsp/lib_foc.c | 100 +++++++++ libs/libdsp/lib_misc.c | 429 ++++++++++++++++++++++++++++++++++++ libs/libdsp/lib_observer.c | 348 +++++++++++++++++++++++++++++ libs/libdsp/lib_pid.c | 282 ++++++++++++++++++++++++ libs/libdsp/lib_svm.c | 340 ++++++++++++++++++++++++++++ libs/libdsp/lib_transform.c | 150 +++++++++++++ tools/Directories.mk | 6 + tools/FlatLibs.mk | 6 + tools/KernelLibs.mk | 6 + tools/LibTargets.mk | 6 + tools/ProtectedLibs.mk | 6 + 16 files changed, 2072 insertions(+) create mode 100644 include/dsp.h create mode 100644 libs/libdsp/Kconfig create mode 100644 libs/libdsp/Makefile create mode 100644 libs/libdsp/README.txt create mode 100644 libs/libdsp/lib_foc.c create mode 100644 libs/libdsp/lib_misc.c create mode 100644 libs/libdsp/lib_observer.c create mode 100644 libs/libdsp/lib_pid.c create mode 100644 libs/libdsp/lib_svm.c create mode 100644 libs/libdsp/lib_transform.c diff --git a/Kconfig b/Kconfig index 23193b6bdb..cae994645e 100644 --- a/Kconfig +++ b/Kconfig @@ -1686,6 +1686,10 @@ source libs/libc/Kconfig source libs/libxx/Kconfig endmenu +menu "DSP Library" +source libs/libdsp/Kconfig +endmenu + menu "Application Configuration" source "$APPSDIR/Kconfig" endmenu diff --git a/include/dsp.h b/include/dsp.h new file mode 100644 index 0000000000..e455429222 --- /dev/null +++ b/include/dsp.h @@ -0,0 +1,285 @@ +/**************************************************************************** + * include/dsp.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_DSP_H +#define __INCLUDE_DSP_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* 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) + +/* 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))) + * + * 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(val, sample, filter) val -= (filter * (val - sample)) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* This structure represents phase angle. + * Besides angle value it also stores sine and cosine values for given angle. + */ + +struct phase_angle_s +{ + float angle; /* Phase angle in radians <0, 2PI> */ + float sin; /* Phase angle sine */ + float cos; /* Phase angle cosine */ +}; + +typedef struct phase_angle_s phase_angle_t; + +/* Float number saturaton */ + +struct float_sat_s +{ + float min; /* Lower limit */ + float max; /* Upper limit */ +}; + +typedef struct float_sat_s float_sat_t; + +/* PI/PID controler state structure */ + +struct pid_controller_s +{ + float out; /* Controller output */ + float_sat_t sat; /* Output saturation */ + float err; /* Current error value */ + float err_prev; /* Previous error value */ + float KP; /* Proportional coefficient */ + float KI; /* Integral coefficient */ + float KD; /* Derivative coefficient */ + float part[3] /* 0 - proporitonal part + * 1 - integral part + * 2 - derivative part + */ +}; + +typedef struct pid_controller_s pid_controller_t; + +/* This structure represents the ABC frame (3 phase vector) */ + +struct abc_frame_s +{ + float a; /* A component */ + float b; /* B component */ + float c; /* C component */ +}; + +typedef struct abc_frame_s abc_frame_t; + +/* This structure represents the alpha-beta frame (2 phase vector) */ + +struct ab_frame_s +{ + float a; /* Alpha component */ + float b; /* Beta component */ +}; + +typedef struct ab_frame_s ab_frame_t; + +/* This structure represent the direct-quadrature frame */ + +struct dq_frame_s +{ + float d; /* Driect component */ + float q; /* Quadrature component */ +}; + +typedef struct dq_frame_s dq_frame_t; + +/* Space Vector Modulation data for 3-phase system */ + +struct svm3_state_s +{ + uint8_t sector; /* Current space vector sector */ + float d_u; /* Duty cycle for phase U */ + float d_v; /* Duty cycle for phase V */ + float d_w; /* Duty cycle for phase W */ +}; + +/* Common motor observer structure */ + +struct motor_observer_s +{ + float angle; /* Estimated observer angle */ + float speed; /* Estimated observer speed */ + float per; /* Observer execution period */ + + /* There are different types of motor observers which different + * sets of private data. + */ + + void *so; /* Speed estimation observer data */ + void *ao; /* Angle estimation observer data */ +}; + +/* Motor Sliding Mode Observer private data */ + +struct motor_observer_smo_s +{ + float k_slide; /* Bang-bang controller gain */ + float err_max; /* Linear mode threashold */ + float F_gain; /* Current observer F gain (1-Ts*R/L) */ + float G_gain; /* Current observer G gain (Ts/L) */ + float emf_lp_filter1; /* Adaptive first low pass EMF filter */ + float emf_lp_filter2; /* Adaptive second low pass EMF filter */ + ab_frame_t emf; /* Estimated back-EMF */ + ab_frame_t z; /* Correction factor */ + ab_frame_t i_est; /* Estimated idq current */ + ab_frame_t v_err; /* v_err = v_ab - emf */ + ab_frame_t i_err; /* i_err = i_est - i_dq */ + ab_frame_t sign; /* Bang-bang controller sign */ +}; + +/* 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_s +{ + uint8_t poles; /* Number of the motor poles */ + float res; /* Phase-to-neutral temperature compensated resistance */ + float ind; /* Average phase-to-neutral inductance */ + + float res_base; /* Phase-to-neutral base resistance */ + float res_alpha; /* Temperature coefficient of resistance */ + float res_temp_ref; /* Reference temperature of alpha */ +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* Math functions */ + +float fast_sin(float angle); +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); +void vector2d_saturate(FAR float *x, FAR float *y, float max); +void dq_saturate(FAR dq_frame_t *dq, float max); + +/* 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_saturation_set(FAR pid_controller_t *pid, float min, float max); +void pi_saturation_set(FAR pid_controller_t *pid, float min, float max); +float pi_controller(FAR pid_controller_t *pid, float err); +float pid_controller(FAR pid_controller_t *pid, float err); + +/* Transformation functions */ + +void clarke_transform(FAR abc_frame_t *abc, FAR ab_frame_t *ab); +void inv_clarke_transform(FAR ab_frame_t *ab, FAR abc_frame_t *abc); +void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab, + FAR dq_frame_t *dq); +void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq, + FAR ab_frame_t *ab); +void 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(FAR struct svm3_state_s *s, FAR ab_frame_t *ab); + +/* 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); + +/* BLDC/PMSM motor observers */ + +void motor_observer_init(FAR struct motor_observer_s *observer, + FAR void *ao, FAR void *so, float per); + +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, + FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab, + FAR struct motor_phy_params_s *phy); + + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +#endif /* __INCLUDE_DSP_H */ diff --git a/libs/libdsp/Kconfig b/libs/libdsp/Kconfig new file mode 100644 index 0000000000..f14ff5fa44 --- /dev/null +++ b/libs/libdsp/Kconfig @@ -0,0 +1,10 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config LIBDSP + bool "Digital Signal Processing Library" + default n + ---help--- + Enable build for various DSP functions diff --git a/libs/libdsp/Makefile b/libs/libdsp/Makefile new file mode 100644 index 0000000000..c98ee9b752 --- /dev/null +++ b/libs/libdsp/Makefile @@ -0,0 +1,88 @@ +############################################################################ +# libdsp/Makefile +# +# 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. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +DELIM ?= $(strip /) + +ASRCS = +CSRCS = +DEPPATH := --dep-path . +VPATH := . + +ifeq ($(CONFIG_LIBDSP),y) +CSRCS += lib_pid.c +CSRCS += lib_svm.c +CSRCS += lib_transform.c +CSRCS += lib_observer.c +CSRCS += lib_foc.c +CSRCS += lib_misc.c +endif + +AOBJS = $(ASRCS:.S=$(OBJEXT)) +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +BIN ?= libdsp$(LIBEXT) + +all: $(BIN) +.PHONY: depend clean distclean + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +$(BIN): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(DEPPATH) "$(CC)" -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, $(BIN)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep diff --git a/libs/libdsp/README.txt b/libs/libdsp/README.txt new file mode 100644 index 0000000000..7432031690 --- /dev/null +++ b/libs/libdsp/README.txt @@ -0,0 +1,6 @@ +libdsp +====== + +This directory contains various DSP functions. + +At the moment you will find here mainly functions related to BLDC/PMSM control. diff --git a/libs/libdsp/lib_foc.c b/libs/libdsp/lib_foc.c new file mode 100644 index 0000000000..75b84dea80 --- /dev/null +++ b/libs/libdsp/lib_foc.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * control/lib_foc.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 + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_current_control + * + * Description: + * 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 + * + * 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) +{ + dq_frame_t idq_err; + + /* Get dq current error */ + + idq_err.d = idq_ref->d - idq->d; + idq_err.q = idq_ref->q - idq->q; + + /* Update regulators saturation */ + + pi_saturation_set(id_pid, sat->min, sat->max); + pi_saturation_set(iq_pid, sat->min, sat->max); + + /* PI controller for d-current (flux loop) */ + + v_dq->d = pi_controller(id_pid, idq_err.d); + + /* PI controller for q-current (torque loop) */ + + v_dq->q = pi_controller(iq_pid, idq_err.q); +} diff --git a/libs/libdsp/lib_misc.c b/libs/libdsp/lib_misc.c new file mode 100644 index 0000000000..4a50b7df8d --- /dev/null +++ b/libs/libdsp/lib_misc.c @@ -0,0 +1,429 @@ +/**************************************************************************** + * control/lib_misc.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 + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: f_saturate + * + * Description: + * Saturate float number + * + * Input Parameters: + * val - pointer to float number + * min - lower limit + * max - upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void f_saturate(FAR float *val, float min, float max) +{ + if (*val < min) + { + *val = min; + } + + else if (*val > max) + { + *val = max; + } +} + +/**************************************************************************** + * Name: vector2d_saturate + * + * 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(FAR float *x, FAR float *y, float max) +{ + float mag = 0.0; + float tmp = 0.0; + + /* Get vector magnitude */ + + mag = sqrtf(*x * *x + *y * *y); + + if (mag < (float)1e-10) + { + mag = (float)1e-10; + } + + if (mag > max) + { + /* Saturate vector */ + + tmp = max / mag; + *x *= tmp; + *y *= tmp; + } +} + +/**************************************************************************** + * Name: dq_saturate + * + * 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(FAR dq_frame_t *dq, float max) +{ + vector2d_saturate(&dq->d, &dq->q, max); +} + +/**************************************************************************** + * Name: fast_sin + * + * Description: + * Fast sin calculation + * + * Reference: http://lab.polygonal.de/?p=205 + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated sine value + * + ****************************************************************************/ + +float fast_sin(float angle) +{ + float sin = 0.0; + float n1 = 1.27323954; + float n2 = 0.405284735; + + /* Normalize angle */ + + angle_norm_2pi(&angle, -M_PI_F, M_PI_F); + + /* Get estiamte sine value from quadratic equation */ + + if (angle < 0.0) + { + sin = n1 * angle + n2 * angle * angle; + } + else + { + sin = n1 * angle - n2 * angle * angle; + } + + return sin; +} + +/**************************************************************************** + * Name:fast_cos + * + * Description: + * Fast cos calculation + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated cosine value + * + ****************************************************************************/ + +float fast_cos(float angle) +{ + /* Get cosine value from sine sin(x + PI/2) = cos(x) */ + + return fast_sin(angle + M_PI_2_F); +} + + +/**************************************************************************** + * Name: fast_sin2 + * + * Description: + * Fast sin calculation with better accuracy + * + * Reference: http://lab.polygonal.de/?p=205 + * + * Input Parameters: + * angle + * + * Returned Value: + * Return estimated sine value + * + ****************************************************************************/ + +float fast_sin2(float angle) +{ + float sin = 0.0; + float n1 = 1.27323954; + float n2 = 0.405284735; + float n3 = 0.225; + + /* Normalize angle */ + + angle_norm_2pi(&angle, -M_PI_F, M_PI_F); + + /* Get estiamte sine value from quadratic equation and do more */ + + if (angle < 0.0) + { + sin = n1 * angle + n2 * angle * angle; + + if (sin < 0) + { + sin = n3 * (sin *(-sin) - sin) + sin; + } + else + { + sin = n3 * (sin * sin - sin) + sin; + } + } + else + { + sin = n1 * angle - n2 * angle * angle; + + if (sin < 0) + { + sin = n3 * (sin *(-sin) - sin) + sin; + } + else + { + sin = n3 * (sin * sin - sin) + sin; + } + } + + return sin; +} + +/**************************************************************************** + * Name:fast_cos2 + * + * Description: + * Fast cos calculation with better accuracy + * + * Input Parameters: + * angle - (in) + * + * Returned Value: + * Return estimated cosine value + * + ****************************************************************************/ + +float fast_cos2(float angle) +{ + /* Get cosine value from sine sin(x + PI/2) = cos(x) */ + + return fast_sin2(angle + M_PI_2_F); +} + +/**************************************************************************** + * Name: fast_atan2 + * + * 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 + * + ****************************************************************************/ + +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; + + /* Get absolute value of y and add some small number to prevent 0/0 */ + + abs_y = fabsf(y)+(float)1e-10; + + /* Calculate angle */ + + if (x >= 0.0) + { + r = (x - abs_y) / (x + abs_y); + rsq = r * r; + angle = ((n1 * rsq) - n2) * r + (float)(M_PI_F / 4.0); + } + else + { + r = (x + abs_y) / (abs_y - x); + rsq = r * r; + angle = ((n1 * rsq) - n2) * r + (float)(3.0 * M_PI_F / 4.0); + } + + /* Get angle sign */ + + if (y < 0.0) + { + angle = -angle; + } + else + { + angle = angle; + } + + return angle; +} + +/**************************************************************************** + * Name: angle_norm + * + * 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(FAR float *angle, float per, float bottom, float 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 + * + * 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(FAR float *angle, float bottom, float top) +{ + angle_norm(angle, 2*M_PI_F, bottom, top); +} + +/**************************************************************************** + * Name: phase_angle_update + * + * 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(FAR struct phase_angle_s *angle, float val) +{ + /* Normalize angle to <0.0, 2PI> */ + + angle_norm_2pi(&val, 0.0, 2*M_PI_F); + + /* Update structure */ + + angle->angle = val; + angle->sin = fast_sin(val); + angle->cos = fast_cos(val); +} diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c new file mode 100644 index 0000000000..0097b3bace --- /dev/null +++ b/libs/libdsp/lib_observer.c @@ -0,0 +1,348 @@ +/**************************************************************************** + * control/lib_observer.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 +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + + +/**************************************************************************** + * Name: motor_observer_init + * + * Description: + * Initialize motor observer + * + * Input Parameters: + * observer - pointer to the common observer data + * ao - pointer to the angle specific observer data + * so - pointer to the speed specific observer data + * per - observer execution period + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_observer_init(FAR struct motor_observer_s *observer, + FAR void *ao, FAR void *so, float per) +{ + DEBUGASSERT(observer != NULL); + DEBUGASSERT(ao != NULL); + DEBUGASSERT(so != NULL); + DEBUGASSERT(per > 0.0); + + /* Set observer period */ + + observer->per = per; + + /* Connect angle estimation observer data */ + + observer->ao = ao; + + /* Connect speed estimation observer data */ + + observer->so = so; +} + +/**************************************************************************** + * Name: motor_observer_smo_init + * + * Description: + * Initialize motor sliding mode observer. + * + * Input Parameters: + * smo - pointer to the sliding mode observer private data + * kslide - SMO gain + * err_max - linear region upper limit + * + * Returned Value: + * None + * + ****************************************************************************/ + +void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo, + float kslide, + float err_max) +{ + DEBUGASSERT(smo != NULL); + DEBUGASSERT(kslide > 0.0); + DEBUGASSERT(err_max > 0.0); + + /* Initialize structure */ + + smo->k_slide = kslide; + smo->err_max = err_max; +} + +/**************************************************************************** + * Name: motor_observer_smo + * + * Description: + * One step of the SMO observer. + * REFERENCE: http://ww1.microchip.com/downloads/en/AppNotes/01078B.pdf + * + * Below some theoretical backgrounds about SMO. + * + * The digitalized motor model can be represent as: + * + * d(i_s.)/dt = (-R/L)*i_s. + (1/L)*(v_s - e_s. - z) + * + * We compare estimated current (i_s.) with measured current (i_s): + * + * err = i_s. - i_s + * + * and get correction factor (z): + * + * sign = sing(err) + * z = sign*K_SLIDE + * + * Once the digitalized model is compensated, we estimate BEMF (e_s.) by + * filtering z: + * + * e_s. = low_pass(z) + * + * The estimated BEMF is filtered once again and used to approximate the + * motor angle: + * + * e_filtered_s. = low_pass(e_s.) + * theta = arctan(-e_alpha/e_beta) + * + * The estimated theta is phase-shifted due to low pass filtration, so we + * need some phase compensation. More details below. + * + * where: + * v_s - phase input voltage vector + * i_s. - estimated phase current vector + * i_s - phase current vector + * e_s. - estimated phase BEMF vector + * R - motor winding resistance + * L - motor winding inductance + * 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 + * + * 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) +{ + FAR struct motor_observer_smo_s *smo = + (FAR struct motor_observer_smo_s *)observer->ao; + FAR ab_frame_t *emf = &smo->emf; + 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; + + /* 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; + + /* Saturate F gain */ + + if (smo->F_gain < 0) + { + smo->F_gain = 0.0; + } + + /* Saturate G gain */ + + if (smo->G_gain > 0.999) + { + smo->G_gain = 0.999; + } + + /* Configure low pass filters + * + * We tune low-pass filters to achieve cutoff frequency equal to + * 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 + * + * 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) + * + * filter = T * (2*PI) * f_c + * filter = T * omega_m * poles + * + * 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 + * + */ + + smo->emf_lp_filter1 = observer->per * observer->speed * phy->poles; + smo->emf_lp_filter2 = smo->emf_lp_filter1; + + /* Get voltage error: v_err = v_ab - emf */ + + v_err->a = v_ab->a - emf->a; + v_err->b = v_ab->b - emf->b; + + /* Estimate stator current */ + + i_est->a = smo->F_gain * i_est->a + smo->G_gain * (v_err->a - z->a); + i_est->b = smo->F_gain * i_est->b + smo->G_gain * (v_err->b - z->b); + + /* Get motor current errror */ + + i_err->a = i_ab->a - i_est->a; + i_err->b = i_ab->b - i_est->b; + + /* Slide-mode controller */ + + sign->a = (i_err->a > 0 ? 1.0 : -1.0); + sign->b = (i_err->b > 0 ? 1.0 : -1.0); + + /* Get current error absolute value - just multiply value with its sign */ + + i_err_a_abs = i_err->a * sign->a; + i_err_b_abs = i_err->b * sign->b; + + /* Calculate new output correction factor voltage */ + + if (i_err_a_abs < smo->err_max) + { + /* Enter linear region if error is small enough */ + + z->a = i_err->a * smo->k_slide / smo->err_max; + } + else + { + /* Non-linear region */ + + z->a = sign->a * smo->k_slide; + } + + if (i_err_b_abs < smo->err_max) + { + /* Enter linear region if error is small enough */ + + z->b = i_err->b * smo->k_slide / smo->err_max; + } + else + { + /* Non-linear region */ + + z->b = sign->b * smo->k_slide; + } + + /* Filter z to obtain estimated emf */ + + LP_FILTER(emf->a, z->a, smo->emf_lp_filter1); + LP_FILTER(emf->b, z->b, smo->emf_lp_filter1); + + /* 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); + + /* Estimate phase angle according to: + * emf_a = -|emf| * sin(th) + * emf_b = |emf| * cos(th) + * th = atan2(-emf_a, emf->b) + */ + + angle = fast_atan2(-emf->a, emf->b); + +#if 1 + /* Some assertions + * TODO: simplyfy + */ + + 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; +#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. + */ + + angle = angle -M_PI_F; + + /* Normalize angle to range <0, 2PI> */ + + angle_norm_2pi(&angle, 0.0, 2*M_PI_F); + + /* Store estimated angle in observer data*/ + + observer->angle = angle; +} diff --git a/libs/libdsp/lib_pid.c b/libs/libdsp/lib_pid.c new file mode 100644 index 0000000000..0f937559d3 --- /dev/null +++ b/libs/libdsp/lib_pid.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * control/lib_pid.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 + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pid_controller_init + * + * 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(FAR pid_controller_t *pid, float KP, float KI, + float KD) +{ + /* Reset controller data */ + + memset(pid, 0, sizeof(pid_controller_t)); + + /* Copy controller parameters */ + + pid->KP = KP; + pid->KI = KI; + pid->KD = KD; +} + +/**************************************************************************** + * Name: pi_controller_init + * + * 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(FAR pid_controller_t *pid, float KP, float KI) +{ + /* Reset controller data */ + + memset(pid, 0, sizeof(pid_controller_t)); + + /* Copy controller parameters */ + + pid->KP = KP; + pid->KI = KI; + pid->KD = 0.0; +} + +/**************************************************************************** + * Name: pid_saturation_set + * + * 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(FAR pid_controller_t* pid, float min, float max) +{ + pid->sat.max = max; + pid->sat.min = min; +} + +/**************************************************************************** + * Name: pi_saturation_set + * + * 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(FAR pid_controller_t* pid, float min, float max) +{ + pid_saturation_set(pid, min, max); +} + +/**************************************************************************** + * Name: pi_controller + * + * 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. + * + ****************************************************************************/ + +float pi_controller(FAR pid_controller_t *pid, float err) +{ + /* Store error in controller structure */ + + pid->err = err; + + /* Get proportional part */ + + pid->part[0] = pid->KP * err; + + /* Get intergral part */ + + pid->part[1] += pid->KI * err; + + /* Add proportional, integral */ + + pid->out = pid->part[0] + pid->part[1]; + + /* Saturate output only if not in PID calculation and if some limits are set */ + + if (pid->sat.max != pid->sat.min && pid->KD == 0.0) + { + if (pid->out > pid->sat.max) + { + /* Limit output to the upper limit */ + + pid->out = pid->sat.max; + + /* Integral anti-windup - reset integral part */ + + if (err > 0) + { + pid->part[1] = 0; + } + } + else if (pid->out < pid->sat.min) + { + /* Limit output to the lower limit */ + + pid->out = pid->sat.min; + + /* Integral anti-windup - reset integral part */ + + if (err < 0) + { + pid->part[1] = 0; + } + } + } + + /* Return regulator output */ + + return pid->out; +} + +/**************************************************************************** + * Name: pid_controller + * + * 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. + * + ****************************************************************************/ + +float pid_controller(FAR pid_controller_t *pid, float err) +{ + /* Get PI output */ + + pi_controller(pid, err); + + /* Get derivative part */ + + pid->part[2] = pid->KD * (err - pid->err_prev); + + /* Add diverative part to the PI part */ + + pid->out += pid->part[2]; + + /* Store current error */ + + pid->err_prev = err; + + /* Saturate output if limits are set */ + + if (pid->sat.max != pid->sat.min) + { + 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_svm.c b/libs/libdsp/lib_svm.c new file mode 100644 index 0000000000..1b58aaede4 --- /dev/null +++ b/libs/libdsp/lib_svm.c @@ -0,0 +1,340 @@ +/**************************************************************************** + * control/lib_svm.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 + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: svm3_sector_get + * + * Description: + * Get current sector for space vector modulation. + * + * Input Parameters: + * s - (in/out) pointer to the SVM state data + * + * Returned Value: + * None + * + ****************************************************************************/ + +static uint8_t svm3_sector_get(FAR abc_frame_t *ijk) +{ + uint8_t sector = 0; + float i = ijk->a; + float j = ijk->b; + float 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.0) + { + if (i <= 0.0) + { + sector = 2; + } + else + { + if (j <= 0.0) + { + sector = 6; + } + else + { + sector = 1; + } + } + } + else + { + if (i <= 0.0) + { + if (j <= 0.0) + { + sector = 4; + } + else + { + sector = 3; + } + } + else + { + sector = 5; + } + } + + /* Return SVM sector */ + + return sector; +} + +/**************************************************************************** + * Name: svm3_duty_calc + * + * Description: + * Calculate duty cycles for space vector modulation. + * + * Input Parameters: + * s - (in/out) pointer to the SVM state data + * + * Returned Value: + * None + * +****************************************************************************/ + +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; + + /* 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 */ + + DEBUGASSERT(0); + break; + } + } + + /* Get null vector time */ + + T0 = (float)1.0 - T1 - T2; + + /* Calculate duty cycle for 3 phase */ + + switch (s->sector) + { + case 1: + { + s->d_u = T1 + T2 + T0/2; + s->d_v = T2 + T0/2; + s->d_w = T0/2; + break; + } + case 2: + { + s->d_u = T1 + T0/2; + s->d_v = T1 + T2 + T0/2; + s->d_w = T0/2; + break; + } + case 3: + { + s->d_u = T0/2; + s->d_v = T1 + T2 + T0/2; + s->d_w = T2 + T0/2; + break; + } + case 4: + { + s->d_u = T0/2; + s->d_v = T1 + T0/2; + s->d_w = T1 + T2 + T0/2; + break; + } + case 5: + { + s->d_u = T2 + T0/2; + s->d_v = T0/2; + s->d_w = T1 + T2 + T0/2; + break; + } + case 6: + { + s->d_u = T1 + T2 + T0/2; + s->d_v = T0/2; + s->d_w = T1 + T0/2; + break; + } + default: + { + /* We should not get here */ + + DEBUGASSERT(0); + break; + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: svm3 + * + * Description: + * One step of the space vector modulation. + * + * 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 <0.0 - 1.0> range + * + * REFERENCE: + * https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download (32-34) + * + ****************************************************************************/ + +void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab) +{ + 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. + */ + + ijk.a = -v_ab->b/2 + 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; + + /* Get vector sector */ + + s->sector = svm3_sector_get(&ijk); + + /* Get duty cycle */ + + svm3_duty_calc(s, &ijk); +} diff --git a/libs/libdsp/lib_transform.c b/libs/libdsp/lib_transform.c new file mode 100644 index 0000000000..2cb844d37f --- /dev/null +++ b/libs/libdsp/lib_transform.c @@ -0,0 +1,150 @@ +/**************************************************************************** + * control/lib_transform.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 + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: Clarke transform (abc frame -> ab frame) + * + * Description: + * 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(FAR abc_frame_t *abc, + FAR ab_frame_t *ab) +{ + ab->a = abc->a; + ab->b = ONE_BY_SQRT3_F*abc->a + TWO_BY_SQRT3_F*abc->b; +} + +/**************************************************************************** + * Name: Inverse Clarke transform (ab frame -> abc frame) + * + * Description: + * 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(FAR ab_frame_t *ab, + FAR abc_frame_t *abc) +{ + /* Assume non-power-invariant transform and balanced system */ + + abc->a = ab->a; + abc->b = -ab->a/2 + SQRT3_BY_TWO_F*ab->b; + abc->c = -abc->a - abc->b; +} + +/**************************************************************************** + * Name: Park transform (ab frame -> dq frame) + * + * Description: + * 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(FAR phase_angle_t *angle, + FAR ab_frame_t *ab, + FAR dq_frame_t *dq) +{ + dq->d = angle->cos * ab->a + angle->sin * ab->b; + dq->q = angle->cos * ab->b - angle->sin * ab->a; +} + +/**************************************************************************** + * Name: Inverse Park transform (dq frame -> ab frame) + * + * Description: + * 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(FAR phase_angle_t *angle, + FAR dq_frame_t *dq, + FAR ab_frame_t *ab) +{ + ab->a = angle->cos * dq->d - angle->sin * dq->q; + ab->b = angle->cos * dq->q + angle->sin * dq->d; +} diff --git a/tools/Directories.mk b/tools/Directories.mk index b4e1f20995..90db265044 100644 --- a/tools/Directories.mk +++ b/tools/Directories.mk @@ -148,6 +148,12 @@ else OTHERDIRS += wireless endif +ifeq ($(CONFIG_LIBDSP),y) +NONFSDIRS += libs$(DELIM)libdsp +else +OTHERDIRS += libs$(DELIM)libdsp +endif + # CLEANDIRS are the directories that will clean in. These are # all directories that we know about. # KERNDEPDIRS are the directories in which we will build target dependencies. diff --git a/tools/FlatLibs.mk b/tools/FlatLibs.mk index 1058a6c85a..4602a0c8ea 100644 --- a/tools/FlatLibs.mk +++ b/tools/FlatLibs.mk @@ -127,6 +127,12 @@ ifeq ($(CONFIG_HAVE_CXX),y) NUTTXLIBS += staging$(DELIM)$(LIBXX)$(LIBEXT) endif +# Add DSP library + +ifeq ($(CONFIG_LIBDSP),y) +NUTTXLIBS += staging$(DELIM)libdsp$(LIBEXT) +endif + # Export all libraries EXPORTLIBS = $(NUTTXLIBS) diff --git a/tools/KernelLibs.mk b/tools/KernelLibs.mk index bfbd18535f..b77e5a8503 100644 --- a/tools/KernelLibs.mk +++ b/tools/KernelLibs.mk @@ -122,6 +122,12 @@ ifeq ($(CONFIG_HAVE_CXX),y) NUTTXLIBS += staging$(DELIM)$(LIBXX)$(LIBEXT) endif +# Add DSP library + +ifeq ($(CONFIG_LIBDSP),y) +NUTTXLIBS += staging$(DELIM)libdsp$(LIBEXT) +endif + # Export only the user libraries EXPORTLIBS = $(USERLIBS) diff --git a/tools/LibTargets.mk b/tools/LibTargets.mk index 598d0cef22..f05a7d6e97 100644 --- a/tools/LibTargets.mk +++ b/tools/LibTargets.mk @@ -130,6 +130,12 @@ $(ARCH_SRC)$(DELIM)libarch$(LIBEXT): context staging$(DELIM)libarch$(LIBEXT): $(ARCH_SRC)$(DELIM)libarch$(LIBEXT) $(Q) $(call INSTALL_LIB,$<,$@) +libs$(DELIM)libdsp$(DELIM)libdsp$(LIBEXT): context + $(Q) $(MAKE) -C libs$(DELIM)libdsp TOPDIR="$(TOPDIR)" libdsp$(LIBEXT) KERNEL=y EXTRADEFINES=$(KDEFINE) + +staging$(DELIM)libdsp$(LIBEXT): libs$(DELIM)libdsp$(DELIM)libdsp$(LIBEXT) + $(Q) $(call INSTALL_LIB,$<,$@) + # Special case syscall$(DELIM)libstubs$(LIBEXT): context diff --git a/tools/ProtectedLibs.mk b/tools/ProtectedLibs.mk index 5a25626f81..1ee16b4641 100644 --- a/tools/ProtectedLibs.mk +++ b/tools/ProtectedLibs.mk @@ -129,6 +129,12 @@ ifeq ($(CONFIG_HAVE_CXX),y) NUTTXLIBS += staging$(DELIM)$(LIBXX)$(LIBEXT) endif +# Add DSP library + +ifeq ($(CONFIG_LIBDSP),y) +NUTTXLIBS += staging$(DELIM)libdsp$(LIBEXT) +endif + # Export only the user libraries EXPORTLIBS = $(USERLIBS)