blob: 79a3ec9024df4acab12e9fc253e061ac55395367 [file] [log] [blame]
/****************************************************************************
* 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 <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
#if !defined(CONFIG_LIBM) && !defined(CONFIG_ARCH_MATH_H)
# error math.h not defined!
#endif
/* Phase rotation direction */
#define DIR_CW (1.0f)
#define DIR_CCW (-1.0f)
/* Some math constants *********************************************************/
#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 ***************************************************************/
/****************************************************************************
* Name: LP_FILTER
*
* Description:
* Simple single-pole digital low pass filter:
* Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n)))
*
* filter - (0.0 - 1.0) where 1.0 gives unfiltered values
* filter = T * (2*PI) * f_c
*
* phase shift = -arctan(f_in/f_c)
*
* T - period at which the digital filter is being calculated
* f_in - input frequency of the filter
* f_c - cutoff frequency of the filter
*
* REFERENCE: https://www.embeddedrelated.com/showarticle/779.php
*
****************************************************************************/
#define LP_FILTER(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
****************************************************************************/
/* 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;
/* 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
{
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 */
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 */
struct motor_observer_s
{
float angle; /* Estimated observer angle */
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.
*/
void *so; /* Speed estimation observer data */
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
{
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 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 */
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 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 */
};
/****************************************************************************
* Public Functions
****************************************************************************/
#undef EXTERN
#if defined(__cplusplus)
#define EXTERN extern "C"
extern "C"
{
#else
#define EXTERN extern
#endif
/* 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);
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_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);
/* 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);
/* 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_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 *o,
FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab,
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)
}
#endif
#endif /* __INCLUDE_DSP_H */