nuttx/nuttx-mirror
1
2
Fork 0
mirror of https://github.com/apache/nuttx.git synced 2025-01-13 06:18:40 +08:00
Code Issues Projects Releases Packages Wiki Activity Actions

Merged in raiden00/nuttx (pull request #648)

Browse source 
libdsp: initial commit

* libdsp: initial commit

* libdsp: cosmetics

Approved-by: Gregory Nutt <gnutt@nuttx.org>
This commit is contained in:
Mateusz Szafoni 2018-05-30 12:36:06 +00:00 committed by Gregory Nutt
parent a0c663edc9
commit b5ec5349b0
16 changed files with 2072 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
Kconfig
View file

@ -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

285
include/dsp.h Normal file
View file

@ -0,0 +1,285 @@
/****************************************************************************
* include/dsp.h
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <nuttx/compiler.h>
#include <stdint.h>
#include <math.h>
/****************************************************************************
* 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 */

10
libs/libdsp/Kconfig Normal file
View file

@ -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

88
libs/libdsp/Makefile Normal file
View file

@ -0,0 +1,88 @@
############################################################################
# libdsp/Makefile
#
# Copyright (C) 2018 Gregory Nutt. All rights reserved.
# Author: Mateusz Szafoni <raiden00@railab.me>
#
# 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

6
libs/libdsp/README.txt Normal file
View file

@ -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.

100
libs/libdsp/lib_foc.c Normal file
View file

@ -0,0 +1,100 @@
/****************************************************************************
* control/lib_foc.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <dsp.h>
/****************************************************************************
* 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);
}

429
libs/libdsp/lib_misc.c Normal file
View file

@ -0,0 +1,429 @@
/****************************************************************************
* control/lib_misc.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <dsp.h>
/****************************************************************************
* 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);
}

348
libs/libdsp/lib_observer.c Normal file
View file

@ -0,0 +1,348 @@
/****************************************************************************
* control/lib_observer.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <stddef.h>
#include <assert.h>
#include <dsp.h>
/****************************************************************************
* 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;
}

282
libs/libdsp/lib_pid.c Normal file
View file

@ -0,0 +1,282 @@
/****************************************************************************
* control/lib_pid.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <string.h>
#include <dsp.h>
/****************************************************************************
* 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;
}

340
libs/libdsp/lib_svm.c Normal file
View file

@ -0,0 +1,340 @@
/****************************************************************************
* control/lib_svm.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <assert.h>
#include <dsp.h>
/****************************************************************************
* 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);
}

150
libs/libdsp/lib_transform.c Normal file
View file

@ -0,0 +1,150 @@
/****************************************************************************
* control/lib_transform.c
*
* Copyright (C) 2018 Gregory Nutt. All rights reserved.
* Author: Mateusz Szafoni <raiden00@railab.me>
*
* 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 <dsp.h>
/****************************************************************************
* 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;
}

6
tools/Directories.mk
View file

@ -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.

6
tools/FlatLibs.mk
View file

@ -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)

6
tools/KernelLibs.mk
View file

@ -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)

6
tools/LibTargets.mk
View file

@ -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

6
tools/ProtectedLibs.mk
View file

@ -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)