sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merged in raiden00/nuttx (pull request #668)

Browse Source 
Master

* libdsp: initial commit

* libdsp: cosmetics

* stm32f334-disco/buckboost: use a PID controller from libdsp

* stm32_adc.h: fix typo

* stm32_dac.c: set OUTEN bit for DAC1CH2 and DAC2CH1

* stm32_hrtim: cosmetic changes

* power/motor: direction parameter is now int8 + add overload fault

* libdsp: all floats with f-sufix

    libdsp: add precision option for library

    libdsp: add debug option for library and assertions in functions

    libdsp: add current samples correction for SVM3

    libds: add some motor control specific functions

    libdsp: add basic speed observer

    libdsp: fix phase shift in SMO observer

    libdsp: add more logic to FOC

    config/sim/dsptest: add dsptest configuration

* libdsp/lib_motor.c: remove unused comparation

* libdsp/lib_observer.c: update some comments

Approved-by: GregoryN <gnutt@nuttx.org>
This commit is contained in:
Mateusz Szafoni 2018-07-07 17:04:57 +00:00 committed by GregoryN
parent 3cd1ccfa48
commit 8416d9a966
17 changed files with 1507 additions and 179 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
arch/arm/src/stm32/stm32_adc.h
View File

@ -1464,7 +1464,7 @@
#if defined(CONFIG_STM32_HRTIM_ADC1_TRG2)
# define ADC1_JEXTSEL_VALUE ADC1_JEXTSEL_HRTTRG2
#elif defined(CONFIG_STM32_HRTIM_ADC1_TRG3)
#elif defined(CONFIG_STM32_HRTIM_ADC1_TRG4)
# define ADC1_JEXTSEL_VALUE ADC1_JEXTSEL_HRTTRG4
#else
# define ADC1_JEXTSEL_VALUE 0
@ -1590,7 +1590,7 @@
#if defined(CONFIG_STM32_HRTIM_ADC2_TRG2)
# define ADC2_JEXTSEL_VALUE ADC2_JEXTSEL_HRTTRG2
#elif defined(CONFIG_STM32_HRTIM_ADC2_TRG3)
#elif defined(CONFIG_STM32_HRTIM_ADC2_TRG4)
# define ADC2_JEXTSEL_VALUE ADC2_JEXTSEL_HRTTRG4
#else
# define ADC2_JEXTSEL_VALUE 0

15
arch/arm/src/stm32/stm32_dac.c
View File

@ -1007,7 +1007,20 @@ static int dac_send(FAR struct dac_dev_s *dev, FAR struct dac_msg_s *msg)
/* Enable DAC Channel */
stm32_dac_modify_cr(chan, 0, DAC_CR_EN);
#if defined(CONFIG_STM32_STM32F33XX)
/* For STM32F33XX we have to set BOFF/OUTEN bit for DAC1CH2 and DAC2CH1
* REVISIT: what if we connect DAC internally with comparator ?
*/
if (chan->intf > 0)
{
stm32_dac_modify_cr(chan, 0, DAC_CR_EN|DAC_CR_BOFF);
}
else
#endif
{
stm32_dac_modify_cr(chan, 0, DAC_CR_EN);
}
#ifdef HAVE_DMA
if (chan->hasdma)

2
arch/arm/src/stm32/stm32_hrtim.c
View File

@ -254,7 +254,7 @@
# define HRTIM_TIME_MODE 0
#endif
#if defined(CONFIG_STM32_HRTIM_TIMA) && !defined( HRTIM_TIMA_UPDATE)
#if defined(CONFIG_STM32_HRTIM_TIMA) && !defined(HRTIM_TIMA_UPDATE)
# warning "HRTIM_TIMA_UPDATE is not set. Set the default value 0"
# define HRTIM_TIMA_UPDATE 0
#endif

2
arch/arm/src/stm32/stm32_hrtim.h
View File

@ -705,6 +705,7 @@ enum stm32_hrtim_chopper_freq_e
enum stm32_hrtim_adc_trq13_e
{
HRTIM_ADCTRG13_NONE = 0, /* No trigger */
HRTIM_ADCTRG13_MC1 = (1 << 0), /* Trigger on Master Compare 1 */
HRTIM_ADCTRG13_MC2 = (1 << 1), /* Trigger on Master Compare 2 */
HRTIM_ADCTRG13_MC3 = (1 << 2), /* Trigger on Master Compare 3 */
@ -749,6 +750,7 @@ enum stm32_hrtim_adc_trq13_e
enum stm32_hrtim_adc_trq24_e
{
HRTIM_ADCTRG24_NONE = 0, /* No trigger */
HRTIM_ADCTRG24_MC1 = (1 << 0), /* Trigger on Master Compare 1 */
HRTIM_ADCTRG24_MC2 = (1 << 1), /* Trigger on Master Compare 2 */
HRTIM_ADCTRG24_MC3 = (1 << 2), /* Trigger on Master Compare 3 */

33
configs/sim/dsptest/defconfig Normal file
View File

@ -0,0 +1,33 @@
CONFIG_ARCH="sim"
CONFIG_ARCH_BOARD="sim"
CONFIG_ARCH_BOARD_SIM=y
CONFIG_ARCH_SIM=y
CONFIG_BUILTIN=y
CONFIG_DEBUG_FEATURES=y
CONFIG_DEBUG_SYMBOLS=y
CONFIG_DEBUG_ASSERTIONS=y
CONFIG_DISABLE_POLL=y
CONFIG_DISABLE_POSIX_TIMERS=y
CONFIG_EXAMPLES_DSPTEST=y
CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_INPUT=y
CONFIG_LIBDSP=y
CONFIG_LIBDSP_DEBUG=y
CONFIG_LIBDSP_PRECISION=2
CONFIG_LIBM=y
CONFIG_LIB_BOARDCTL=y
CONFIG_MAX_TASKS=64
CONFIG_MQ_MAXMSGSIZE=64
CONFIG_NFILE_DESCRIPTORS=32
CONFIG_PREALLOC_MQ_MSGS=4
CONFIG_PTHREAD_STACK_DEFAULT=8192
CONFIG_SCHED_WAITPID=y
CONFIG_SDCLONE_DISABLE=y
CONFIG_START_DAY=6
CONFIG_START_MONTH=10
CONFIG_START_YEAR=2011
CONFIG_SYSTEM_READLINE=y
CONFIG_TESTING_UNITY=y
CONFIG_TESTING_UNITY_OUTPUT_COLOR=y
CONFIG_USERMAIN_STACKSIZE=4096
CONFIG_USER_ENTRYPOINT="dsptest_main"

14
drivers/power/motor.c
View File

@ -427,6 +427,20 @@ static int motor_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
goto errout;
}
#ifdef CONFIG_MOTOR_HAVE_DIRECTION
/* Check direction configuration */
if (params->direction != MOTOR_DIR_CCW &&
params->direction != MOTOR_DIR_CW)
{
pwrerr("ERROR: invalid direction value %d\n",
params->direction);
ret = -EPERM;
goto errout;
}
#endif
#ifdef CONFIG_MOTOR_HAVE_POSITION
/* Check position configuration */

255
include/dsp.h
View File

@ -41,40 +41,107 @@
****************************************************************************/
#include <nuttx/compiler.h>
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include <assert.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#ifndef CONFIG_BUILD_FLAT
# error "Only flat build supported for now"
#endif
/* Disable DEBUGASSERT macro if LIBDSP debug is not enabled */
#ifdef CONFIG_LIBDSP_DEBUG
# ifndef CONFIG_DEBUG_ASSERTIONS
# warning "Need CONFIG_DEBUG_ASSERTIONS to work properly"
# endif
#else
# undef DEBUGASSERT
# define DEBUGASSERT(x)
#endif
#ifndef CONFIG_LIBDSP_PRECISION
# define CONFIG_LIBDSP_PRECISION 0
#endif
/* Phase rotation direction */
#define DIR_CW (1.0f)
#define DIR_CCW (-1.0f)
/* Some math constants *********************************************************/
#define SQRT3_BY_TWO_F ((float)0.866025)
#define SQRT3_BY_THREE_F ((float)0.57735)
#define ONE_BY_SQRT3_F ((float)0.57735)
#define TWO_BY_SQRT3_F ((float)1.15470)
#define SQRT3_BY_TWO_F (0.866025f)
#define SQRT3_BY_THREE_F (0.57735f)
#define ONE_BY_SQRT3_F (0.57735f)
#define TWO_BY_SQRT3_F (1.15470f)
/* Some lib constants **********************************************************/
/* Motor electrical angle is in range 0.0 to 2*PI */
#define MOTOR_ANGLE_E_MAX (2.0f*M_PI_F)
#define MOTOR_ANGLE_E_MIN (0.0f)
#define MOTOR_ANGLE_E_RANGE (MOTOR_ANGLE_E_MAX - MOTOR_ANGLE_E_MIN)
/* Motor mechanical angle is in range 0.0 to 2*PI */
#define MOTOR_ANGLE_M_MAX (2.0f*M_PI_F)
#define MOTOR_ANGLE_M_MIN (0.0f)
#define MOTOR_ANGLE_M_RANGE (MOTOR_ANGLE_M_MAX - MOTOR_ANGLE_M_MIN)
/* Some useful macros ***************************************************************/
/* Simple single-pole digital low pass filter:
* Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n)))
/****************************************************************************
* Name: LP_FILTER
*
* filter - (0.0 - 1.0) where 1.0 gives unfiltered values
* filter = T * (2*PI) * f_c
* Description:
* Simple single-pole digital low pass filter:
* Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n)))
*
* phase shift = -arctan(f_in/f_c)
* filter - (0.0 - 1.0) where 1.0 gives unfiltered values
* filter = T * (2*PI) * f_c
*
* T - period at which the digital filter is being calculated
* f_in - input frequency of the filter
* f_c - cutoff frequency of the filter
* phase shift = -arctan(f_in/f_c)
*
* T - period at which the digital filter is being calculated
* f_in - input frequency of the filter
* f_c - cutoff frequency of the filter
*
* REFERENCE: https://www.embeddedrelated.com/showarticle/779.php
*
*/
****************************************************************************/
#define LP_FILTER(val, sample, filter) val -= (filter * (val - sample))
/****************************************************************************
* Name: SVM3_BASE_VOLTAGE_GET
*
* Description:
* Get maximum voltage for SVM3 without overmodulation
*
* Notes:
* max possible phase voltage for 3-phase power inwerter:
* Vd = (2/3)*Vdc
* max phase reference voltage according to SVM modulation diagram:
* Vrefmax = Vd * cos(30*) = SQRT3_BY_2 * Vd
* which give us:
* Vrefmax = SQRT3_BY_3 * Vdc
*
* Vdc - bus voltage
*
****************************************************************************/
#define SVM3_BASE_VOLTAGE_GET(vbus) (vbus * SQRT3_BY_THREE_F)
/****************************************************************************
* Public Types
****************************************************************************/
@ -92,6 +159,28 @@ struct phase_angle_s
typedef struct phase_angle_s phase_angle_t;
/* This structure stores motor angles and corresponding sin and cos values
*
* th_el = th_m * pole_pairs
* th_m = th_el/pole_pairs
*
* where:
* th_el - motor electrical angle
* th_m - motor mechanical angle
* pole_pairs - motor pole pairs
*
* NOTE: pole_pairs = poles_total/2
*/
struct motor_angle_s
{
phase_angle_t angle_el; /* Electrical angle */
float anglem; /* Mechanical angle in radians <0, 2PI> */
float one_by_p; /* Aux variable */
uint8_t p; /* Number of the motor pole pairs */
int8_t i; /* Pole counter */
};
/* Float number saturaton */
struct float_sat_s
@ -160,6 +249,17 @@ struct svm3_state_s
float d_u; /* Duty cycle for phase U */
float d_v; /* Duty cycle for phase V */
float d_w; /* Duty cycle for phase W */
float d_max; /* Duty cycle max */
float d_min; /* Duty cycle min */
};
/* Motor open-loop control data */
struct openloop_data_s
{
float max; /* Open-loop max speed */
float angle; /* Open-loop current angle normalized to <0.0, 2PI> */
float per; /* Open-loop control execution period */
};
/* Common motor observer structure */
@ -170,6 +270,11 @@ struct motor_observer_s
float speed; /* Estimated observer speed */
float per; /* Observer execution period */
float angle_err; /* Observer angle error.
* This can be used to gradually eliminate
* error between openloop angle and observer angle
*/
/* There are different types of motor observers which different
* sets of private data.
*/
@ -178,6 +283,27 @@ struct motor_observer_s
void *ao; /* Angle estimation observer data */
};
/* Speed observer division method data */
struct motor_sobserver_div_s
{
float angle_diff; /* Mechanical angle difference */
float angle_acc; /* Accumulated mechanical angle */
float angle_prev; /* Previous mechanical angle */
float one_by_dt; /* Frequency of observer execution */
float cntr; /* Sample counter */
float samples; /* Number of samples for observer */
float filter; /* Low-pass filter for final omega */
};
/* Speed observer PLL method data */
#if 0
struct motor_sobserver_pll_s
{
/* TODO */
};
#endif
/* Motor Sliding Mode Observer private data */
struct motor_observer_smo_s
@ -189,6 +315,7 @@ struct motor_observer_smo_s
float emf_lp_filter1; /* Adaptive first low pass EMF filter */
float emf_lp_filter2; /* Adaptive second low pass EMF filter */
ab_frame_t emf; /* Estimated back-EMF */
ab_frame_t emf_f; /* Fitlered estimated back-EMF */
ab_frame_t z; /* Correction factor */
ab_frame_t i_est; /* Estimated idq current */
ab_frame_t v_err; /* v_err = v_ab - emf */
@ -203,13 +330,42 @@ struct motor_observer_smo_s
struct motor_phy_params_s
{
uint8_t poles; /* Number of the motor poles */
float res; /* Phase-to-neutral temperature compensated resistance */
float ind; /* Average phase-to-neutral inductance */
uint8_t p; /* Number of the motor pole pairs */
float res; /* Phase-to-neutral temperature compensated
* resistance
*/
float res_base; /* Phase-to-neutral base resistance */
float res_alpha; /* Temperature coefficient of resistance */
float res_temp_ref; /* Reference temperature of alpha */
float ind; /* Average phase-to-neutral inductance */
float one_by_ind; /* Inverse phase-to-neutral inductance */
};
/* Field oriented control (FOC) data
* REVISIT:
*/
struct foc_data_s
{
abc_frame_t v_abc; /* Voltage in ABC frame */
ab_frame_t v_ab; /* Voltage in alpha-beta frame */
dq_frame_t v_dq; /* Voltage in dq frame */
ab_frame_t v_ab_mod; /* Modulation voltage normalized to
* magnitude (0.0, 1.0)
*/
abc_frame_t i_abc; /* Current in ABC frame */
ab_frame_t i_ab; /* Current in apha-beta frame*/
dq_frame_t i_dq; /* Current in dq frame */
dq_frame_t i_dq_err; /* DQ current error */
dq_frame_t i_dq_ref; /* Currrent dq reference frame */
pid_controller_t id_pid; /* Current d-axis component PI controller */
pid_controller_t iq_pid; /* Current q-axis component PI controller */
float vdq_mag_max; /* Maximum dq voltage magnitude */
float vab_mod_scale; /* Voltage alpha-beta modulation scale */
};
/****************************************************************************
@ -223,16 +379,25 @@ float fast_sin2(float angle);
float fast_cos(float angle);
float fast_cos2(float angle);
float fast_atan2(float y, float x);
void f_saturate(FAR float *val, float min, float max);
float vector2d_mag(float x, float y);
void vector2d_saturate(FAR float *x, FAR float *y, float max);
void dq_saturate(FAR dq_frame_t *dq, float max);
float dq_mag(FAR dq_frame_t *dq);
/* PID controller functions */
void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI, float KD);
void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI);
void pid_controller_init(FAR pid_controller_t *pid,
float KP, float KI, float KD);
void pi_controller_init(FAR pid_controller_t *pid,
float KP, float KI);
void pid_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pi_saturation_set(FAR pid_controller_t *pid, float min, float max);
void pid_integral_reset(FAR pid_controller_t *pid);
void pi_integral_reset(FAR pid_controller_t *pid);
float pi_controller(FAR pid_controller_t *pid, float err);
float pid_controller(FAR pid_controller_t *pid, float err);
@ -244,34 +409,72 @@ void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab,
FAR dq_frame_t *dq);
void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq,
FAR ab_frame_t *ab);
/* Phase angle related functions */
void angle_norm(FAR float *angle, float per, float bottom, float top);
void angle_norm_2pi(FAR float *angle, float bottom, float top);
void phase_angle_update(FAR struct phase_angle_s *angle, float val);
/* 3-phase system space vector modulation*/
void svm3_init(FAR struct svm3_state_s *s, float min, float max);
void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *ab);
void svm3_current_correct(FAR struct svm3_state_s *s,
int32_t *c0, int32_t *c1, int32_t *c2);
/* Field Oriented control */
void foc_current_control(FAR pid_controller_t *id_pid,
FAR pid_controller_t *iq_pid,
FAR dq_frame_t *idq_ref,
FAR dq_frame_t *idq,
FAR float_sat_t *sat,
FAR dq_frame_t *v_dq);
void foc_vbase_update(FAR struct foc_data_s *foc, float vbase);
void foc_idq_ref_set(FAR struct foc_data_s *data, float d, float q);
void foc_init(FAR struct foc_data_s *data,
float id_kp, float id_ki, float iq_kp, float iq_ki);
void foc_process(FAR struct foc_data_s *foc,
FAR abc_frame_t *i_abc,
FAR phase_angle_t *angle);
/* BLDC/PMSM motor observers */
void motor_observer_init(FAR struct motor_observer_s *observer,
FAR void *ao, FAR void *so, float per);
float motor_observer_speed_get(FAR struct motor_observer_s *o);
float motor_observer_angle_get(FAR struct motor_observer_s *o);
void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
float kslide, float err_max);
void motor_observer_smo(FAR struct motor_observer_s *observer,
void motor_observer_smo(FAR struct motor_observer_s *o,
FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab,
FAR struct motor_phy_params_s *phy);
FAR struct motor_phy_params_s *phy, float dir);
void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
uint8_t samples, float filer, float per);
void motor_sobserver_div(FAR struct motor_observer_s *o,
float angle, float dir);
/* Motor openloop control */
void motor_openloop_init(FAR struct openloop_data_s *op,
float max, float per);
void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir);
float motor_openloop_angle_get(FAR struct openloop_data_s *op);
/* Motor angle */
void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p);
void motor_angle_e_update(FAR struct motor_angle_s *angle,
float angle_new,float dir);
void motor_angle_m_update(FAR struct motor_angle_s *angle,
float angle_new,float dir);
float motor_angle_m_get(FAR struct motor_angle_s *angle);
float motor_angle_e_get(FAR struct motor_angle_s *angle);
/* Motor physical parameters functions */
void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
float res, float ind);
void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
float res_alpha, float res_temp_ref);
#undef EXTERN
#if defined(__cplusplus)

21
include/nuttx/power/motor.h
View File

@ -98,11 +98,20 @@ enum motor_fault_e
{
MOTOR_FAULT_OVERCURRENT = (1 << 0), /* Over-current Fault */
MOTOR_FAULT_OVERVOLTAGE = (1 << 1), /* Over-voltage Fault */
MOTOR_FAULT_OVERPOWER = (1 << 2), /* Over-power Fault */
MOTOR_FAULT_OVERPOWER = (1 << 2), /* Over-power Fault (electrical) */
MOTOR_FAULT_OVERTEMP = (1 << 3), /* Over-temperature Fault */
MOTOR_FAULT_LOCKED = (1 << 4), /* Motor locked Fault */
MOTOR_FAULT_INVAL_PARAM = (1 << 5), /* Invalid parameter Fault */
MOTOR_FAULT_OTHER = (1 << 6) /* Other Fault */
MOTOR_FAULT_OVERLOAD = (1 << 4), /* Motor overload Fault (mechanical) */
MOTOR_FAULT_LOCKED = (1 << 5), /* Motor locked Fault */
MOTOR_FAULT_INVAL_PARAM = (1 << 6), /* Invalid parameter Fault */
MOTOR_FAULT_OTHER = (1 << 7) /* Other Fault */
};
/* Motor direction */
enum motor_direction_e
{
MOTOR_DIR_CCW = -1,
MOTOR_DIR_CW = 1
};
/* This structure contains feedback data from motor driver */
@ -191,7 +200,7 @@ struct motor_params_s
* parameter during run-time.
*/
#ifdef CONFIG_MOTOR_HAVE_DIRECTION
bool direction; /* Motor movement direction. We do not
int8_t direction; /* Motor movement direction. We do not
* support negative values for parameters,
* so this flag can be used to allow movement
* in the positive and negative direction in
@ -214,7 +223,7 @@ struct motor_params_s
float acceleration; /* Motor acceleration */
#endif
#ifdef CONFIG_MOTOR_HAVE_DECELERATION
float deceleartion; /* Motor deceleration */
float deceleration; /* Motor deceleration */
#endif
};

20
libs/libdsp/Kconfig
View File

@ -8,3 +8,23 @@ config LIBDSP
default n
---help---
Enable build for various DSP functions
config LIBDSP_DEBUG
bool "Libdsp debugging"
depends on LIBDSP
default n
---help---
Enable debugging for libdsp. This option enables additional parameters
checking. It can drastically reduce performance and be potentially
dangerous to hardware, so it should be used carefully (probably only
at an early stage of application development).
config LIBDSP_PRECISION
int "Libdsp precision [0/1/2]"
default 0
---help---
Whith this option we can select libdsp precision for
some of calculations. There are 3 available options:
0 - the fastest calculation but the lowest precision
1 - a little better precision than above, but slowest
2 - the most accuracte but the slowest one, use standard math functions.

1
libs/libdsp/Makefile
View File

@ -49,6 +49,7 @@ CSRCS += lib_transform.c
CSRCS += lib_observer.c
CSRCS += lib_foc.c
CSRCS += lib_misc.c
CSRCS += lib_motor.c
endif
AOBJS = $(ASRCS:.S=$(OBJEXT))

236
libs/libdsp/lib_foc.c
View File

@ -37,6 +37,9 @@
* Included Files
****************************************************************************/
#include <string.h>
#include <stdbool.h>
#include <dsp.h>
/****************************************************************************
@ -47,10 +50,6 @@
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_current_control
*
@ -58,43 +57,228 @@
* This function implements FOC current control algorithm.
*
* Input Parameters:
* id_pid - (in) pointer to the direct current PI controller data
* iq_pid - (in) pointer to the quadrature current PI controller data
* idq_ref - (in) dq current reference
* i_dq - (in) dq current
* vdq_min - (in) lower regulator limit
* vdq_max - (in) upper regulator limit
* v_dq - (out) pointer to the dq voltage frame
* foc - (in/out) pointer to the FOC data
*
* Returned Value:
* None
*
****************************************************************************/
void foc_current_control(FAR pid_controller_t *id_pid,
FAR pid_controller_t *iq_pid,
FAR dq_frame_t *idq_ref,
FAR dq_frame_t *idq,
FAR float_sat_t *sat,
FAR dq_frame_t *v_dq)
static void foc_current_control(FAR struct foc_data_s *foc)
{
dq_frame_t idq_err;
FAR pid_controller_t *id_pid = &foc->id_pid;
FAR pid_controller_t *iq_pid = &foc->iq_pid;
FAR dq_frame_t *v_dq = &foc->v_dq;
/* Get dq current error */
idq_err.d = idq_ref->d - idq->d;
idq_err.q = idq_ref->q - idq->q;
foc->i_dq_err.d = foc->i_dq_ref.d - foc->i_dq.d;
foc->i_dq_err.q = foc->i_dq_ref.q - foc->i_dq.q;
/* Update regulators saturation */
pi_saturation_set(id_pid, sat->min, sat->max);
pi_saturation_set(iq_pid, sat->min, sat->max);
/* NOTE: PI controllers saturation is updated in foc_vdq_mag_max_set() */
/* PI controller for d-current (flux loop) */
v_dq->d = pi_controller(id_pid, idq_err.d);
v_dq->d = pi_controller(id_pid, foc->i_dq_err.d);
/* PI controller for q-current (torque loop) */
v_dq->q = pi_controller(iq_pid, idq_err.q);
v_dq->q = pi_controller(iq_pid, foc->i_dq_err.q);
/* Saturate voltage DQ vector.
* The maximum DQ voltage magnitude depends on the maximum possible
* phase voltage and the maximum supported duty cycle.
*/
dq_saturate(v_dq, foc->vdq_mag_max);
}
/****************************************************************************
* Name: foc_vab_mod_scale_set
*
* Description:
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* scale - (in) scaling factor for alpha-beta voltage
*
* Returned Value:
* None
*
****************************************************************************/
static void foc_vab_mod_scale_set(FAR struct foc_data_s *foc, float scale)
{
foc->vab_mod_scale = scale;
}
/****************************************************************************
* Name: foc_vdq_mag_max_set
*
* Description:
* Set maximum dq voltage vector magnitude
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* max - (in) maximum dq voltage magnitude
*
* Returned Value:
* None
*
****************************************************************************/
static void foc_vdq_mag_max_set(FAR struct foc_data_s *foc, float max)
{
foc->vdq_mag_max = max;
/* Update regulators saturation */
pi_saturation_set(&foc->id_pid, -foc->vdq_mag_max, foc->vdq_mag_max);
pi_saturation_set(&foc->iq_pid, -foc->vdq_mag_max, foc->vdq_mag_max);
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_init
*
* Description:
* Initialize FOC controller
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* id_kp - (in) KP for d current
* id_ki - (in) KI for d current
* iq_kp - (in) KP for q current
* iq_ki - (in) KI for q current
*
* Returned Value:
* None
*
****************************************************************************/
void foc_init(FAR struct foc_data_s *foc,
float id_kp, float id_ki, float iq_kp, float iq_ki)
{
/* Reset data */
memset(foc, 0, sizeof(struct foc_data_s));
/* Initialize PI current d component */
pi_controller_init(&foc->id_pid, id_kp, id_ki);
/* Initialize PI current q component */
pi_controller_init(&foc->iq_pid, iq_kp, iq_ki);
}
/****************************************************************************
* Name: foc_idq_ref_set
*
* Description:
* Set dq reference current vector
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* d - (in) reference d current
* q - (in) reference q current
*
* Returned Value:
* None
*
****************************************************************************/
void foc_idq_ref_set(FAR struct foc_data_s *foc, float d, float q)
{
foc->i_dq_ref.d = d;
foc->i_dq_ref.q = q;
}
/****************************************************************************
* Name: foc_vbase_update
*
* Description:
* Update base voltage for FOC controller
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* vbase - (in) base voltage for FOC
*
* Returned Value:
* None
*
****************************************************************************/
void foc_vbase_update(FAR struct foc_data_s *foc, float vbase)
{
float scale = 0.0f;
float mag_max = 0.0f;
/* Only if voltage is valid */
if (vbase >= 0.0f)
{
scale = 1.0f/vbase;
mag_max = vbase;
}
foc_vab_mod_scale_set(foc, scale);
foc_vdq_mag_max_set(foc, mag_max);
}
/****************************************************************************
* Name: foc_process
*
* Description:
* Process FOC (Field Oriented Control)
*
* Input Parameters:
* foc - (in/out) pointer to the FOC data
* i_abc - (in) pointer to the ABC current frame
* angle - (in) pointer to the phase angle data
*
* Returned Value:
* None
*
* TODO: add some reference and a brief description of the FOC
*
****************************************************************************/
void foc_process(FAR struct foc_data_s *foc,
FAR abc_frame_t *i_abc,
FAR phase_angle_t *angle)
{
DEBUGASSERT(foc != NULL);
DEBUGASSERT(i_abc != NULL);
DEBUGASSERT(angle != NULL);
/* Copy ABC current to foc data */
foc->i_abc.a = i_abc->a;
foc->i_abc.b = i_abc->b;
foc->i_abc.c = i_abc->c;
/* Convert abc current to alpha-beta current */
clarke_transform(&foc->i_abc, &foc->i_ab);
/* Convert alpha-beta current to dq current */
park_transform(angle, &foc->i_ab, &foc->i_dq);
/* Run FOC current control (current dq -> voltage dq) */
foc_current_control(foc);
/* Inverse Park tranform (voltage dq -> voltage alpha-beta) */
inv_park_transform(angle, &foc->v_dq, &foc->v_ab);
/* Normalize the alpha-beta voltage to get the alpha-beta modulation voltage */
foc->v_ab_mod.a = foc->v_ab.a * foc->vab_mod_scale;
foc->v_ab_mod.b = foc->v_ab.b * foc->vab_mod_scale;
}

108
libs/libdsp/lib_misc.c
View File

@ -80,6 +80,26 @@ void f_saturate(FAR float *val, float min, float max)
}
}
/****************************************************************************
* Name: vector2d_mag
*
* Description:
* Get 2D vector magnitude.
*
* Input Parameters:
* x - (in) vector x component
* y - (in) vector y component
*
* Returned Value:
* Return 2D vector magnitude
*
****************************************************************************/
float vector2d_mag(float x, float y)
{
return sqrtf(x * x + y * y);
}
/****************************************************************************
* Name: vector2d_saturate
*
@ -98,16 +118,16 @@ void f_saturate(FAR float *val, float min, float max)
void vector2d_saturate(FAR float *x, FAR float *y, float max)
{
float mag = 0.0;
float tmp = 0.0;
float mag = 0.0f;
float tmp = 0.0f;
/* Get vector magnitude */
mag = sqrtf(*x * *x + *y * *y);
mag = vector2d_mag(*x, *y);
if (mag < (float)1e-10)
if (mag < 1e-10f)
{
mag = (float)1e-10;
mag = 1e-10f;
}
if (mag > max)
@ -120,6 +140,25 @@ void vector2d_saturate(FAR float *x, FAR float *y, float max)
}
}
/****************************************************************************
* Name: dq_mag
*
* Description:
* Get DQ vector magnitude.
*
* Input Parameters:
* dq - (in/out) dq frame vector
*
* Returned Value:
* Return dq vector magnitude
*
****************************************************************************/
float dq_mag(FAR dq_frame_t *dq)
{
return vector2d_mag(dq->d, dq->q);
}
/****************************************************************************
* Name: dq_saturate
*
@ -158,9 +197,9 @@ void dq_saturate(FAR dq_frame_t *dq, float max)
float fast_sin(float angle)
{
float sin = 0.0;
float n1 = 1.27323954;
float n2 = 0.405284735;
float sin = 0.0f;
float n1 = 1.27323954f;
float n2 = 0.405284735f;
/* Normalize angle */
@ -168,7 +207,7 @@ float fast_sin(float angle)
/* Get estiamte sine value from quadratic equation */
if (angle < 0.0)
if (angle < 0.0f)
{
sin = n1 * angle + n2 * angle * angle;
}
@ -220,10 +259,10 @@ float fast_cos(float angle)
float fast_sin2(float angle)
{
float sin = 0.0;
float n1 = 1.27323954;
float n2 = 0.405284735;
float n3 = 0.225;
float sin = 0.0f;
float n1 = 1.27323954f;
float n2 = 0.405284735f;
float n3 = 0.225f;
/* Normalize angle */
@ -231,11 +270,11 @@ float fast_sin2(float angle)
/* Get estiamte sine value from quadratic equation and do more */
if (angle < 0.0)
if (angle < 0.0f)
{
sin = n1 * angle + n2 * angle * angle;
if (sin < 0)
if (sin < 0.0f)
{
sin = n3 * (sin *(-sin) - sin) + sin;
}
@ -248,7 +287,7 @@ float fast_sin2(float angle)
{
sin = n1 * angle - n2 * angle * angle;
if (sin < 0)
if (sin < 0.0f)
{
sin = n3 * (sin *(-sin) - sin) + sin;
}
@ -302,35 +341,35 @@ float fast_cos2(float angle)
float fast_atan2(float y, float x)
{
float angle = 0.0;
float abs_y = 0.0;
float rsq = 0.0;
float r = 0.0;
float n1 = 0.1963;
float n2 = 0.9817;
float angle = 0.0f;
float abs_y = 0.0f;
float rsq = 0.0f;
float r = 0.0f;
float n1 = 0.1963f;
float n2 = 0.9817f;
/* Get absolute value of y and add some small number to prevent 0/0 */
abs_y = fabsf(y)+(float)1e-10;
abs_y = fabsf(y)+1e-10f;
/* Calculate angle */
if (x >= 0.0)
if (x >= 0.0f)
{
r = (x - abs_y) / (x + abs_y);
rsq = r * r;
angle = ((n1 * rsq) - n2) * r + (float)(M_PI_F / 4.0);
angle = ((n1 * rsq) - n2) * r + (M_PI_F / 4.0f);
}
else
{
r = (x + abs_y) / (abs_y - x);
rsq = r * r;
angle = ((n1 * rsq) - n2) * r + (float)(3.0 * M_PI_F / 4.0);
angle = ((n1 * rsq) - n2) * r + (3.0f * M_PI_F / 4.0f);
}
/* Get angle sign */
if (y < 0.0)
if (y < 0.0f)
{
angle = -angle;
}
@ -394,7 +433,7 @@ void angle_norm(FAR float *angle, float per, float bottom, float top)
void angle_norm_2pi(FAR float *angle, float bottom, float top)
{
angle_norm(angle, 2*M_PI_F, bottom, top);
angle_norm(angle, 2.0f*M_PI_F, bottom, top);
}
/****************************************************************************
@ -417,13 +456,24 @@ void angle_norm_2pi(FAR float *angle, float bottom, float top)
void phase_angle_update(FAR struct phase_angle_s *angle, float val)
{
DEBUGASSERT(angle != NULL);
/* Normalize angle to <0.0, 2PI> */
angle_norm_2pi(&val, 0.0, 2*M_PI_F);
angle_norm_2pi(&val, 0.0f, 2.0f*M_PI_F);
/* Update structure */
angle->angle = val;
#if CONFIG_LIBDSP_PRECISION == 1
angle->sin = fast_sin2(val);
angle->cos = fast_cos2(val);
#elif CONFIG_LIBDSP_PRECISION == 2
angle->sin = sin(val);
angle->cos = cos(val);
#else
angle->sin = fast_sin(val);
angle->cos = fast_cos(val);
#endif
}

426
libs/libdsp/lib_motor.c Normal file
View File

@ -0,0 +1,426 @@
/****************************************************************************
* control/lib_motor.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
****************************************************************************/
#define POLE_CNTR_THR 0.0f
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: motor_openloop_init
*
* Description:
* Initialize open-loop data
*
* Input Parameters:
* op - (in/out) pointer to the openlooop data structure
* max - (in) maximum openloop speed
* per - (in) period of the open-loop control
*
* Returned Value:
* None
*
****************************************************************************/
void motor_openloop_init(FAR struct openloop_data_s *op, float max, float per)
{
DEBUGASSERT(op != NULL);
DEBUGASSERT(max > 0.0f);
DEBUGASSERT(per > 0.0f);
/* Reset openloop structure */
memset(op, 0, sizeof(struct openloop_data_s));
/* Initialize data */
op->max = max;
op->per = per;
}
/****************************************************************************
* Name: motor_openloop
*
* Description:
* One step of the open-loop control
*
* Input Parameters:
* op - (in/out) pointer to the open-loop data structure
* speed - (in) open-loop speed
* dir - (in) rotation direction
*
* Returned Value:
* None
*
****************************************************************************/
void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir)
{
DEBUGASSERT(op != NULL);
DEBUGASSERT(speed >= 0.0f);
DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
float phase_step = 0.0f;
/* REVISIT: what should we do if speed is greater than max open-loop speed?
* Saturate speed or stop motor ?
*/
if (speed > op->max)
{
speed = op->max;
}
/* Get phase step */
phase_step = dir * speed * op->per;
/* Update open-loop angle */
op->angle += phase_step;
/* Normalize the open-looop angle to 0.0 - 2PI range */
angle_norm_2pi(&op->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX);
}
/****************************************************************************
* Name: motor_openloop_angle_get
*
* Description:
* Get angle from open-loop controller
*
* Input Parameters:
* op - (in/out) pointer to the open-loop data structure
*
* Returned Value:
* Return angle from open-loop controller
*
****************************************************************************/
float motor_openloop_angle_get(FAR struct openloop_data_s *op)
{
DEBUGASSERT(op != NULL);
return op->angle;
}
/* In a multipolar electrical machines, mechanical angle is not equal
* to mechanical angle which can be described as:
*
* electrical angle = (p) * mechanical angle
* where p - number of poles pair
*
*
* electrical
* angle:
*
* poles = 4
* i = 0 1 2 3 0 1 2 3 0
* 2PI ______________________________________________
* /| /| /| /| /| /| /| /| /|
* / | / | / | / | / | / | / | / | / |
* / | / | / | / | / | / | / | / | / |
* 0 /___|/___|/___|/___|/___|/___|/___|/___|/___|/
* . . .
* mechanical . . .
* angle: . . .
* . . .
* 2PI .__________________.___________________.______
* . o o
* . o o o o
* . o o o o
* . o o o o o
* 0 o__________________o___________________o______
*
*/
/****************************************************************************
* Name: motor_angle_init
*
* Description:
* Initialize motor angle structure
*
* Input Parameters:
* angle - (in/out) pointer to the motor angle strucutre
* p - (in) number of the motor pole pairs
*
* Returned Value:
* None
*
****************************************************************************/
void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(p > 0);
/* Reset structure */
memset(angle, 0, sizeof(struct motor_angle_s));
/* Store pole pairs */
angle->p = p;
angle->one_by_p = (float)1.0f/p;
/* Initialize angle with 0.0 */
phase_angle_update(&angle->angle_el, 0.0f);
}
/****************************************************************************
* Name: motor_angle_e_update
*
* Description:
* Update motor angle structure using electrical motor angle.
*
* Input Parameters:
* angle - (in/out) pointer to the motor angle structure
* angle_new - (in) new motor electrical angle in range <0.0, 2PI>
*
* Returned Value:
* None
*
****************************************************************************/
void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
/* Check if we crossed electrical angle boundaries */
if (dir == DIR_CW &&
angle_new - angle->angle_el.angle < -POLE_CNTR_THR)
{
/* For CW direction - previous angle is greater than current angle */
angle->i += 1;
}
else if (dir == DIR_CCW &&
angle_new - angle->angle_el.angle > POLE_CNTR_THR)
{
/* For CCW direction - previous angle is lower than current angle */
angle->i -= 1;
}
/* Reset pole counter if needed */
if (angle->i >= angle->p)
{
angle->i = 0;
}
else if (angle->i < 0)
{
angle->i = angle->p-1;
}
/* Update electrical angle structure */
phase_angle_update(&angle->angle_el, angle_new);
/* Calculate mechanical angle.
* One electrical angle rotation is equal to one mechanical rotation divided
* by number of motor pole pairs.
*/
angle->anglem = (MOTOR_ANGLE_E_RANGE * angle->i +
angle->angle_el.angle) * angle->one_by_p;
/* Normalize mechanical angle to <0, 2PI> and store */
angle_norm_2pi(&angle->anglem, MOTOR_ANGLE_M_MIN, MOTOR_ANGLE_M_MAX);
}
/****************************************************************************
* Name: motor_angle_m_update
*
* Description:
* Update motor angle structure using mechanical motor angle
*
* Input Parameters:
* angle - (in/out) pointer to the motor angle structure
* angle_new - (in) new motor mechanical angle in range <0.0, 2PI>
*
* Returned Value:
* None
*
****************************************************************************/
void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
float angle_el = 0.0f;
/* Store new mechanical angle */
angle->anglem = angle_new;
/* Update pole counter */
angle->i = (uint8_t)(angle->anglem * angle->p/MOTOR_ANGLE_M_MAX);
/* Get electrical angle */
angle_el = angle->anglem * angle->p - MOTOR_ANGLE_E_MAX * angle->i;
/* Update electrical angle structure */
phase_angle_update(&angle->angle_el, angle_el);
}
/****************************************************************************
* Name: motor_angle_m_get
*
* Description:
* Get motor mechanical angle
*
* Input Parameters:
* angle - (in/out) pointer to the motor angle structure
*
* Returned Value:
* Return motor mechanical angle
*
****************************************************************************/
float motor_angle_m_get(FAR struct motor_angle_s *angle)
{
DEBUGASSERT(angle != NULL);
return angle->anglem;
}
/****************************************************************************
* Name: motor_angle_e_get
*
* Description:
* Get motor electrical angle
*
* Input Parameters:
* angle - (in/out) pointer to the motor angle structure
*
* Returned Value:
* Return motor electrical angle
*
****************************************************************************/
float motor_angle_e_get(FAR struct motor_angle_s *angle)
{
DEBUGASSERT(angle != NULL);
return angle->angle_el.angle;
}
/****************************************************************************
* Name: motor_phy_params_init
*
* Description:
* Initialize motor physical parameters
*
* Input Parameters:
* phy - (in/out) pointer to the motor physical parameters
* poles - (in) number of the motor pole pairs
* res - (in) average phase-to-neutral base motor resistance
* (without temperature compensation)
* ind - (in) average phase-to-neutral motor inductance
*
* Returned Value:
* None
*
****************************************************************************/
void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
float res, float ind)
{
DEBUGASSERT(phy != NULL);
phy->p = poles;
phy->res_base = res_base;
phy->ind = ind;
phy->one_by_ind = 1.0f/ind;
/* Initialize with zeros */
phy->res = 0.0f;
phy->res_alpha = 0.0f;
phy->res_temp_ref = 0.0f;
}
/****************************************************************************
* Name: motor_phy_params_temp_set
* Description:
* Initialize motor physical temperature parameters
*
* Input Parameters:
* phy - (in/out) pointer to the motor physical parameters
* res_alpha - (in) temperature coefficient of the winding resistance
* res_temp_ref - (in) reference temperature for alpha
*
* Returned Value:
* None
*
****************************************************************************/
void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
float res_alpha, float res_temp_ref)
{
DEBUGASSERT(phy != NULL);
phy->res_alpha = res_alpha;
phy->res_temp_ref = res_temp_ref;
}

312
libs/libdsp/lib_observer.c
View File

@ -37,15 +37,14 @@
* Included Files
****************************************************************************/
#include <stddef.h>
#include <assert.h>
#include <dsp.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define ANGLE_DIFF_THR M_PI_F
/****************************************************************************
* Private Functions
****************************************************************************/
@ -77,7 +76,11 @@ void motor_observer_init(FAR struct motor_observer_s *observer,
DEBUGASSERT(observer != NULL);
DEBUGASSERT(ao != NULL);
DEBUGASSERT(so != NULL);
DEBUGASSERT(per > 0.0);
DEBUGASSERT(per > 0.0f);
/* Reset observer data */
memset(observer, 0, sizeof(struct motor_observer_s));
/* Set observer period */
@ -113,8 +116,12 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
float err_max)
{
DEBUGASSERT(smo != NULL);
DEBUGASSERT(kslide > 0.0);
DEBUGASSERT(err_max > 0.0);
DEBUGASSERT(kslide > 0.0f);
DEBUGASSERT(err_max > 0.0f);
/* Reset structure */
memset(smo, 0, sizeof(struct motor_observer_smo_s));
/* Initialize structure */
@ -168,50 +175,59 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
* z - output correction factor voltage
*
* Input Parameters:
* observer - (in/out) pointer to the common observer data
* i_ab - (in) inverter alpha-beta current
* v_ab - (in) inverter alpha-beta voltage
* phy - (in) pointer to the motor physical parameters
* o - (in/out) pointer to the common observer data
* i_ab - (in) inverter alpha-beta current
* v_ab - (in) inverter alpha-beta voltage
* phy - (in) pointer to the motor physical parameters
* dir - (in) rotation direction (1.0 for CW, -1.0 for CCW)
*
* Returned Value:
* None
*
****************************************************************************/
void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i_ab,
FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy)
void motor_observer_smo(FAR struct motor_observer_s *o, FAR ab_frame_t *i_ab,
FAR ab_frame_t *v_ab, FAR struct motor_phy_params_s *phy,
float dir)
{
DEBUGASSERT(o != NULL);
DEBUGASSERT(i_ab != NULL);
DEBUGASSERT(v_ab != NULL);
DEBUGASSERT(phy != NULL);
FAR struct motor_observer_smo_s *smo =
(FAR struct motor_observer_smo_s *)observer->ao;
(FAR struct motor_observer_smo_s *)o->ao;
FAR ab_frame_t *emf = &smo->emf;
FAR ab_frame_t *emf_f = &smo->emf_f;
FAR ab_frame_t *z = &smo->z;
FAR ab_frame_t *i_est = &smo->i_est;
FAR ab_frame_t *v_err = &smo->v_err;
FAR ab_frame_t *i_err = &smo->i_err;
FAR ab_frame_t *sign = &smo->sign;
float i_err_a_abs = 0.0;
float i_err_b_abs = 0.0;
float angle = 0.0;
float i_err_a_abs = 0.0f;
float i_err_b_abs = 0.0f;
float angle = 0.0f;
float filter = 0.0f;
/* REVISIT: observer works only when IQ current is high enough */
/* Calculate observer gains */
smo->F_gain = (1 - observer->per*phy->res/phy->ind);
smo->G_gain = observer->per/phy->ind;
smo->F_gain = (1.0f - o->per*phy->res*phy->one_by_ind);
smo->G_gain = o->per*phy->one_by_ind;
/* Saturate F gain */
if (smo->F_gain < 0)
if (smo->F_gain < 0.0f)
{
smo->F_gain = 0.0;
smo->F_gain = 0.0f;
}
/* Saturate G gain */
if (smo->G_gain > 0.999)
if (smo->G_gain > 0.999f)
{
smo->G_gain = 0.999;
smo->G_gain = 0.999f;
}
/* Configure low pass filters
@ -220,27 +236,45 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i
* input singal frequency. This gives us constant phase shift between
* input and outpu signals equals to:
*
* phi = -arctan(f_in/f_c) = -arctan(1) = -PI/2
* phi = -arctan(f_in/f_c) = -arctan(1) = -45deg = -PI/4
*
* Input signal frequency is equal to the frequency of the motor currents,
* which give us:
*
* f_c = omega_e/(2*PI)
* omega_m = omega_e/poles
* f_c = omega_m*poles/(2*PI)
* omega_m = omega_e/pole_pairs
* f_c = omega_m*pole_pairs/(2*PI)
*
* filter = T * (2*PI) * f_c
* filter = T * omega_m * poles
* filter = T * omega_m * pole_pairs
*
* T - [s] period at which the digital filter is being calculated
* f_in - [Hz] input frequency of the filter
* f_c - [Hz] cutoff frequency of the filter
* omega_m - [rad/s] mechanical angular velocity
* omega_e - [rad/s] electrical angular velocity
* T - [s] period at which the digital filter is being calculated
* f_in - [Hz] input frequency of the filter
* f_c - [Hz] cutoff frequency of the filter
* omega_m - [rad/s] mechanical angular velocity
* omega_e - [rad/s] electrical angular velocity
* pole_pairs - pole pairs
*
*/
smo->emf_lp_filter1 = observer->per * observer->speed * phy->poles;
filter = o->per * o->speed * phy->p;
/* Limit SMO filters
* REVISIT: lowest filter limit should depend on minimum speed:
* filter = T * (2*PI) * f_c = T * omega0
*
*/
if (filter >= 1.0f)
{
filter = 0.99f;
}
else if (filter <= 0.0f)
{
filter = 0.005f;
}
smo->emf_lp_filter1 = filter;
smo->emf_lp_filter2 = smo->emf_lp_filter1;
/* Get voltage error: v_err = v_ab - emf */
@ -260,8 +294,8 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i
/* Slide-mode controller */
sign->a = (i_err->a > 0 ? 1.0 : -1.0);
sign->b = (i_err->b > 0 ? 1.0 : -1.0);
sign->a = (i_err->a > 0.0f ? 1.0f : -1.0f);
sign->b = (i_err->b > 0.0f ? 1.0f : -1.0f);
/* Get current error absolute value - just multiply value with its sign */
@ -303,8 +337,8 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i
/* Filter emf one more time before angle stimation */
LP_FILTER(emf->a, emf->a, smo->emf_lp_filter2);
LP_FILTER(emf->b, emf->b, smo->emf_lp_filter2);
LP_FILTER(emf_f->a, emf->a, smo->emf_lp_filter2);
LP_FILTER(emf_f->b, emf->b, smo->emf_lp_filter2);
/* Estimate phase angle according to:
* emf_a = -|emf| * sin(th)
@ -319,29 +353,211 @@ void motor_observer_smo(FAR struct motor_observer_s *observer, FAR ab_frame_t *i
* TODO: simplify
*/
if (angle != angle) angle = 0.0;
if (emf->a != emf->a) emf->a = 0.0;
if (emf->b != emf->b) emf->b = 0.0;
if (z->a != z->a) z->a = 0.0;
if (z->b != z->b) z->b = 0.0;
if (i_est->a != i_est->a) i_est->a = 0.0;
if (i_est->b != i_est->b) i_est->b = 0.0;
if (angle != angle) angle = 0.0f;
if (emf->a != emf->a) emf->a = 0.0f;
if (emf->b != emf->b) emf->b = 0.0f;
if (z->a != z->a) z->a = 0.0f;
if (z->b != z->b) z->b = 0.0f;
if (i_est->a != i_est->a) i_est->a = 0.0f;
if (i_est->b != i_est->b) i_est->b = 0.0f;
#endif
/* Angle compensation.
* Due to low pass filtering we have some delay in estimated phase angle.
*
* Adaptive filters introduced above cause -PI/2 phase shift for each filter.
* We use 2 times filtering which give us constant -PI phase shift.
* Adaptive filters introduced above cause -PI/4 phase shift for each filter.
* We use 2 times filtering which give us constant -PI/2 (-90deg) phase shift.
*/
angle = angle -M_PI_F;
angle = angle + dir * M_PI_2_F;
/* Normalize angle to range <0, 2PI> */
angle_norm_2pi(&angle, 0.0, 2*M_PI_F);
angle_norm_2pi(&angle, 0.0f, 2.0f*M_PI_F);
/* Store estimated angle in observer data*/
observer->angle = angle;
o->angle = angle;
}
/****************************************************************************
* Name: motor_sobserver_div_init
*
* Description:
* Initialize DIV speed observer
*
* Input Parameters:
* so - (in/out) pointer to the DIV speed observer data
* sample - (in) number of mechanical angle samples
* filter - (in) low-pass filter for final omega
* per - (in) speed observer execution period
*
* Returned Value:
* None
*
****************************************************************************/
void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
uint8_t samples,
float filter,
float per)
{
DEBUGASSERT(so != NULL);
DEBUGASSERT(samples > 0);
DEBUGASSERT(filter > 0.0f);
/* Reset observer data */
memset(so, 0, sizeof(struct motor_sobserver_div_s));
/* Store number of samples for DIV observer */
so->samples = samples;
/* Store low-pass filter for DIV observer speed */
so->filter = filter;
/* */
so->one_by_dt = 1.0f/(so->samples * per);
}
/****************************************************************************
* Name: motor_sobserver_div
*
* Description:
* Estimate motor mechanical speed based on motor mechanical angle
* difference.
*
* Input Parameters:
* o - (in/out) pointer to the common observer data
* angle - (in) mechanical angle normalized to <0.0, 2PI>
* dir - (in) mechanical rotation direction. Valid values:
* DIR_CW (1.0f) or DIR_CCW(-1.0f)
*
****************************************************************************/
void motor_sobserver_div(FAR struct motor_observer_s *o,
float angle, float dir)
{
DEBUGASSERT(o != NULL);
DEBUGASSERT(angle >= 0.0f && angle <= 2*M_PI_F);
DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);
FAR struct motor_sobserver_div_s *so =
(FAR struct motor_sobserver_div_s *)o->so;
volatile float omega = 0.0f;
/* Get angle diff */
so->angle_diff = angle - so->angle_prev;
/* Correct angle if we crossed angle boundary
* REVISIT:
*/
if ((dir == DIR_CW && so->angle_diff < -ANGLE_DIFF_THR) ||
(dir == DIR_CCW && so->angle_diff > ANGLE_DIFF_THR))
{
/* Correction sign depends on rotation direction */
so->angle_diff += dir*2*M_PI_F;
}
/* Get absoulte value */
if (so->angle_diff < 0.0f)
{
so->angle_diff = -so->angle_diff;
}
/* Accumulate angle only if sample is valid */
so->angle_acc += so->angle_diff;
/* Increase counter */
so->cntr += 1;
/* Accumulate angle until we get configured number of samples */
if (so->cntr >= so->samples)
{
/* Estimate omega using accumulated angle samples.
* In this case use simple estimation:
*
* omega = delta_theta/delta_time
* speed_now = low_pass(omega)
*
*/
omega = so->angle_acc*so->one_by_dt;
/* Store filtered omega.
*
* REVISIT: cut-off frequency for this filter should be
* (probably) set according to minimum supported omega:
*
* filter = T * (2*PI) * f_c = T * omega0
*
* where:
* omega0 - minimum angular speed
* T - speed estimation period (samples*one_by_dt)
*/
LP_FILTER(o->speed, omega, so->filter);
/* Reset samples counter and accumulated angle */
so->cntr = 0;
so->angle_acc = 0.0f;
}
/* Store current angle as previous angle */
so->angle_prev = angle;
}
/****************************************************************************
* Name: motor_observer_speed_get
*
* Description:
* Get the estmiated motor mechanical speed from the observer
*
* Input Parameters:
* o - (in/out) pointer to the common observer data
*
* Returned Value:
* Return estimated motor mechanical speed from observer
*
****************************************************************************/
float motor_observer_speed_get(FAR struct motor_observer_s *o)
{
DEBUGASSERT(o != NULL);
return o->speed;
}
/****************************************************************************
* Name: motor_observer_angle_get
*
* Description:
* Get the estmiated motor electrical angle from the observer
*
* Input Parameters:
* o - (in/out) pointer to the common observer data
*
* Returned Value:
* Return estimated motor mechanical angle from observer
*
****************************************************************************/
float motor_observer_angle_get(FAR struct motor_observer_s *o)
{
DEBUGASSERT(o != NULL);
return o->angle;
}

55
libs/libdsp/lib_pid.c
View File

@ -37,8 +37,6 @@
* Included Files
****************************************************************************/
#include <string.h>
#include <dsp.h>
/****************************************************************************
@ -70,6 +68,8 @@
void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI,
float KD)
{
DEBUGASSERT(pid != NULL);
/* Reset controller data */
memset(pid, 0, sizeof(pid_controller_t));
@ -100,6 +100,8 @@ void pid_controller_init(FAR pid_controller_t *pid, float KP, float KI,
void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI)
{
DEBUGASSERT(pid != NULL);
/* Reset controller data */
memset(pid, 0, sizeof(pid_controller_t));
@ -108,7 +110,7 @@ void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI)
pid->KP = KP;
pid->KI = KI;
pid->KD = 0.0;
pid->KD = 0.0f;
}
/****************************************************************************
@ -129,8 +131,11 @@ void pi_controller_init(FAR pid_controller_t *pid, float KP, float KI)
*
****************************************************************************/
void pid_saturation_set(FAR pid_controller_t* pid, float min, float max)
void pid_saturation_set(FAR pid_controller_t *pid, float min, float max)
{
DEBUGASSERT(pid != NULL);
DEBUGASSERT(min < max);
pid->sat.max = max;
pid->sat.min = min;
}
@ -150,11 +155,32 @@ void pid_saturation_set(FAR pid_controller_t* pid, float min, float max)
*
****************************************************************************/
void pi_saturation_set(FAR pid_controller_t* pid, float min, float max)
void pi_saturation_set(FAR pid_controller_t *pid, float min, float max)
{
DEBUGASSERT(pid != NULL);
DEBUGASSERT(min < max);
pid_saturation_set(pid, min, max);
}
/****************************************************************************
* Name: pid_integral_reset
****************************************************************************/
void pid_integral_reset(FAR pid_controller_t *pid)
{
pid->part[1] = 0.0f;
}
/****************************************************************************
* Name: pi_integral_reset
****************************************************************************/
void pi_integral_reset(FAR pid_controller_t *pid)
{
pid_integral_reset(pid);
}
/****************************************************************************
* Name: pi_controller
*
@ -172,6 +198,8 @@ void pi_saturation_set(FAR pid_controller_t* pid, float min, float max)
float pi_controller(FAR pid_controller_t *pid, float err)
{
DEBUGASSERT(pid != NULL);
/* Store error in controller structure */
pid->err = err;
@ -188,9 +216,12 @@ float pi_controller(FAR pid_controller_t *pid, float err)
pid->out = pid->part[0] + pid->part[1];
/* Saturate output only if not in PID calculation and if some limits are set */
/* Saturate output only if we are not in a PID calculation and only
* if some limits are set. Saturation for a PID controller are done later
* in PID routine.
*/
if (pid->sat.max != pid->sat.min && pid->KD == 0.0)
if (pid->sat.max != pid->sat.min && pid->KD == 0.0f)
{
if (pid->out > pid->sat.max)
{
@ -200,9 +231,9 @@ float pi_controller(FAR pid_controller_t *pid, float err)
/* Integral anti-windup - reset integral part */
if (err > 0)
if (err > 0.0f)
{
pid->part[1] = 0;
pi_integral_reset(pid);
}
}
else if (pid->out < pid->sat.min)
@ -213,9 +244,9 @@ float pi_controller(FAR pid_controller_t *pid, float err)
/* Integral anti-windup - reset integral part */
if (err < 0)
if (err < 0.0f)
{
pid->part[1] = 0;
pi_integral_reset(pid);
}
}
}
@ -242,6 +273,8 @@ float pi_controller(FAR pid_controller_t *pid, float err)
float pid_controller(FAR pid_controller_t *pid, float err)
{
DEBUGASSERT(pid != NULL);
/* Get PI output */
pi_controller(pid, err);

166
libs/libdsp/lib_svm.c
View File

@ -56,7 +56,7 @@
* Get current sector for space vector modulation.
*
* Input Parameters:
* s - (in/out) pointer to the SVM state data
* ijk - (in) pointer to the auxiliary ABC frame
*
* Returned Value:
* None
@ -146,7 +146,8 @@ static uint8_t svm3_sector_get(FAR abc_frame_t *ijk)
* Calculate duty cycles for space vector modulation.
*
* Input Parameters:
* s - (in/out) pointer to the SVM state data
* s - (in/out) pointer to the SVM state data
* ijk - (in) pointer to the auxiliary ABC frame
*
* Returned Value:
* None
@ -158,9 +159,9 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
float i = ijk->a;
float j = ijk->b;
float k = ijk->c;
float T0 = 0.0;
float T1 = 0.0;
float T2 = 0.0;
float T0 = 0.0f;
float T1 = 0.0f;
float T2 = 0.0f;
/* Determine T1, T2 and T0 based on the sector */
@ -213,7 +214,7 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
/* Get null vector time */
T0 = (float)1.0 - T1 - T2;
T0 = 1.0f - T1 - T2;
/* Calculate duty cycle for 3 phase */
@ -221,44 +222,44 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
{
case 1:
{
s->d_u = T1 + T2 + T0/2;
s->d_v = T2 + T0/2;
s->d_w = T0/2;
s->d_u = T1 + T2 + T0*0.5f;
s->d_v = T2 + T0*0.5f;
s->d_w = T0*0.5f;
break;
}
case 2:
{
s->d_u = T1 + T0/2;
s->d_v = T1 + T2 + T0/2;
s->d_w = T0/2;
s->d_u = T1 + T0*0.5f;
s->d_v = T1 + T2 + T0*0.5f;
s->d_w = T0*0.5f;
break;
}
case 3:
{
s->d_u = T0/2;
s->d_v = T1 + T2 + T0/2;
s->d_w = T2 + T0/2;
s->d_u = T0*0.5f;
s->d_v = T1 + T2 + T0*0.5f;
s->d_w = T2 + T0*0.5f;
break;
}
case 4:
{
s->d_u = T0/2;
s->d_v = T1 + T0/2;
s->d_w = T1 + T2 + T0/2;
s->d_u = T0*0.5f;
s->d_v = T1 + T0*0.5f;
s->d_w = T1 + T2 + T0*0.5f;
break;
}
case 5:
{
s->d_u = T2 + T0/2;
s->d_v = T0/2;
s->d_w = T1 + T2 + T0/2;
s->d_u = T2 + T0*0.5f;
s->d_v = T0*0.5f;
s->d_w = T1 + T2 + T0*0.5f;
break;
}
case 6:
{
s->d_u = T1 + T2 + T0/2;
s->d_v = T0/2;
s->d_w = T1 + T0/2;
s->d_u = T1 + T2 + T0*0.5f;
s->d_v = T0*0.5f;
s->d_w = T1 + T0*0.5f;
break;
}
default:
@ -280,6 +281,7 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
*
* Description:
* One step of the space vector modulation.
* This is most common of SVM with alternate-reverse null vector.
*
* Voltage vector definitions in 3-phase SVM:
*
@ -311,7 +313,13 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
* Input Parameters:
* s - (out) pointer to the SVM data
* v_ab - (in) pointer to the modulation voltage vector in alpha-beta frame,
* normalized to <0.0 - 1.0> range
* normalized to magnitude (0.0 - 1.0)
*
* NOTE: v_ab vector magnitude must be in range <0.0, 1.0> to get correct
* SVM3 results.
*
* REVISIT: not sure how we should handle invalid data from user.
* For now we saturate output duty form SVM.
*
* REFERENCE:
* https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download (32-34)
@ -320,15 +328,18 @@ static void svm3_duty_calc(FAR struct svm3_state_s *s, FAR abc_frame_t *ijk)
void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab)
{
DEBUGASSERT(s != NULL);
DEBUGASSERT(v_ab != NULL);
abc_frame_t ijk;
/* Perform modified inverse Clarke-transformation (alpha,beta) -> (i,j,k)
* to obtain auxliary frame which will be used in further calculations.
* to obtain auxiliary frame which will be used in further calculations.
*/
ijk.a = -v_ab->b/2 + SQRT3_BY_TWO_F*v_ab->a;
ijk.a = -0.5f*v_ab->b + SQRT3_BY_TWO_F*v_ab->a;
ijk.b = v_ab->b;
ijk.c = -v_ab->b/2 - SQRT3_BY_TWO_F*v_ab->a;
ijk.c = -ijk.b - ijk.a;
/* Get vector sector */
@ -337,4 +348,103 @@ void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *v_ab)
/* Get duty cycle */
svm3_duty_calc(s, &ijk);
/* Saturate output from SVM */
f_saturate(&s->d_u, s->d_min, s->d_max);
f_saturate(&s->d_v, s->d_min, s->d_max);
f_saturate(&s->d_w, s->d_min, s->d_max);
}
/****************************************************************************
* Name: svm3_current_correct
*
* Description:
* Correct ADC samples (int32) according to SVM3 state.
* NOTE: This works only with 3 shunt resistors configuration.
*
****************************************************************************/
void svm3_current_correct(FAR struct svm3_state_s *s,
int32_t *c0, int32_t *c1, int32_t *c2)
{
/* Get best ADC samples according to SVM sector.
*
* In SVM phase current can be sampled only in v0 vector state, when lower
* bridge transistors are turned on.
*
* We ignore sample from phase which has the shortest V0 state and
* estimate its value with KCL for motor phases:
* i_a + i_b + i_c = 0
*/
switch (s->sector)
{
case 1:
case 6:
{
/* Sector 1-6: ignore phase 1 */
*c0 = -(*c1 + *c2);
break;
}
case 2:
case 3:
{
/* Sector 2-3: ignore phase 2 */
*c1 = -(*c0 + *c2);
break;
}
case 4:
case 5:
{
/* Sector 4-5: ignore phase 3 */
*c2 = -(*c0 + *c1);
break;
}
default:
{
/* We should not get here. */
*c0 = 0;
*c1 = 0;
*c2 = 0;
break;
}
}
}
/****************************************************************************
* Name: svm3_init
*
* Description:
* Initialize 3-phase SVM data.
*
* Input Parameters:
* s - (in/out) pointer to the SVM state data
* sat - (in)
*
* Returned Value:
* None
*
****************************************************************************/
void svm3_init(FAR struct svm3_state_s *s, float min, float max)
{
DEBUGASSERT(s != NULL);
DEBUGASSERT(max > min);
memset(s, 0, sizeof(struct svm3_state_s));
s->d_max = max;
s->d_min = min;
}

16
libs/libdsp/lib_transform.c
View File

@ -72,6 +72,9 @@
void clarke_transform(FAR abc_frame_t *abc,
FAR ab_frame_t *ab)
{
DEBUGASSERT(abc != NULL);
DEBUGASSERT(ab != NULL);
ab->a = abc->a;
ab->b = ONE_BY_SQRT3_F*abc->a + TWO_BY_SQRT3_F*abc->b;
}
@ -94,10 +97,13 @@ void clarke_transform(FAR abc_frame_t *abc,
void inv_clarke_transform(FAR ab_frame_t *ab,
FAR abc_frame_t *abc)
{
DEBUGASSERT(ab != NULL);
DEBUGASSERT(abc != NULL);
/* Assume non-power-invariant transform and balanced system */
abc->a = ab->a;
abc->b = -ab->a/2 + SQRT3_BY_TWO_F*ab->b;
abc->b = -0.5f*ab->a + SQRT3_BY_TWO_F*ab->b;
abc->c = -abc->a - abc->b;
}
@ -121,6 +127,10 @@ void park_transform(FAR phase_angle_t *angle,
FAR ab_frame_t *ab,
FAR dq_frame_t *dq)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(ab != NULL);
DEBUGASSERT(dq != NULL);
dq->d = angle->cos * ab->a + angle->sin * ab->b;
dq->q = angle->cos * ab->b - angle->sin * ab->a;
}
@ -145,6 +155,10 @@ void inv_park_transform(FAR phase_angle_t *angle,
FAR dq_frame_t *dq,
FAR ab_frame_t *ab)
{
DEBUGASSERT(angle != NULL);
DEBUGASSERT(dq != NULL);
DEBUGASSERT(ab != NULL);
ab->a = angle->cos * dq->d - angle->sin * dq->q;
ab->b = angle->cos * dq->q + angle->sin * dq->d;
}