582 lines
19 KiB
C
582 lines
19 KiB
C
/****************************************************************************
|
|
* include/dsp.h
|
|
*
|
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
* this work for additional information regarding copyright ownership. The
|
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
|
* "License"); you may not use this file except in compliance with the
|
|
* License. You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
|
* License for the specific language governing permissions and limitations
|
|
* under the License.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#ifndef __INCLUDE_DSP_H
|
|
#define __INCLUDE_DSP_H
|
|
|
|
/****************************************************************************
|
|
* Included Files
|
|
****************************************************************************/
|
|
|
|
#include <nuttx/compiler.h>
|
|
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <math.h>
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
/* 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
|
|
# define LIBDSP_DEBUGASSERT(x) DEBUGASSERT(x)
|
|
#else
|
|
# undef LIBDSP_DEBUGASSERT
|
|
# define LIBDSP_DEBUGASSERT(x)
|
|
#endif
|
|
|
|
#ifndef CONFIG_LIBDSP_PRECISION
|
|
# define CONFIG_LIBDSP_PRECISION 0
|
|
#endif
|
|
|
|
/* Phase rotation direction */
|
|
|
|
#define DIR_NONE (0.0f)
|
|
#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 *******************************************************/
|
|
|
|
/* These are defined only in the NuttX math library */
|
|
|
|
#ifndef M_PI_F
|
|
#define M_PI_F ((float)M_PI)
|
|
#endif
|
|
|
|
#ifndef M_PI_2_F
|
|
#define M_PI_2_F ((float)M_PI_2)
|
|
#endif
|
|
|
|
/* 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 inverter:
|
|
* 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_f32_s
|
|
{
|
|
float angle; /* Phase angle in radians <0, 2PI> */
|
|
float sin; /* Phase angle sine */
|
|
float cos; /* Phase angle cosine */
|
|
};
|
|
|
|
typedef struct phase_angle_f32_s phase_angle_f32_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_f32_s
|
|
{
|
|
phase_angle_f32_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_f32_s
|
|
{
|
|
float min; /* Lower limit */
|
|
float max; /* Upper limit */
|
|
};
|
|
|
|
typedef struct float_sat_f32_s float_sat_f32_t;
|
|
|
|
/* PI/PID controller state structure */
|
|
|
|
struct pid_controller_f32_s
|
|
{
|
|
bool aw_en; /* Integral part decay if saturated */
|
|
bool ireset_en; /* Intergral part reset if saturated */
|
|
bool pisat_en; /* PI saturation enabled */
|
|
bool pidsat_en; /* PID saturation enabled */
|
|
bool _res; /* Reserved */
|
|
float out; /* Controller output */
|
|
float_sat_f32_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
|
|
*/
|
|
float KC; /* Integral anti-windup decay coefficient */
|
|
float aw; /* Integral anti-windup decay part */
|
|
};
|
|
|
|
typedef struct pid_controller_f32_s pid_controller_f32_t;
|
|
|
|
/* This structure represents the ABC frame (3 phase vector) */
|
|
|
|
struct abc_frame_f32_s
|
|
{
|
|
float a; /* A component */
|
|
float b; /* B component */
|
|
float c; /* C component */
|
|
};
|
|
|
|
typedef struct abc_frame_f32_s abc_frame_f32_t;
|
|
|
|
/* This structure represents the alpha-beta frame (2 phase vector) */
|
|
|
|
struct ab_frame_f32_s
|
|
{
|
|
float a; /* Alpha component */
|
|
float b; /* Beta component */
|
|
};
|
|
|
|
typedef struct ab_frame_f32_s ab_frame_f32_t;
|
|
|
|
/* This structure represent the direct-quadrature frame */
|
|
|
|
struct dq_frame_f32_s
|
|
{
|
|
float d; /* Driect component */
|
|
float q; /* Quadrature component */
|
|
};
|
|
|
|
typedef struct dq_frame_f32_s dq_frame_f32_t;
|
|
|
|
/* Space Vector Modulation data for 3-phase system */
|
|
|
|
struct svm3_state_f32_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 */
|
|
};
|
|
|
|
/* Motor open-loop control data */
|
|
|
|
struct openloop_data_f32_s
|
|
{
|
|
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_f32_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_f32_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_f32_s
|
|
{
|
|
/* TODO */
|
|
};
|
|
#endif
|
|
|
|
/* Motor Sliding Mode Observer private data */
|
|
|
|
struct motor_observer_smo_f32_s
|
|
{
|
|
float k_slide; /* Bang-bang controller gain */
|
|
float err_max; /* Linear mode threshold */
|
|
float one_by_err_max; /* One by err_max */
|
|
float F; /* Current observer F gain (1-Ts*R/L) */
|
|
float G; /* 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_f32_t emf; /* Estimated back-EMF */
|
|
ab_frame_f32_t emf_f; /* Fitlered estimated back-EMF */
|
|
ab_frame_f32_t z; /* Correction factor */
|
|
ab_frame_f32_t i_est; /* Estimated idq current */
|
|
ab_frame_f32_t v_err; /* v_err = v_ab - emf */
|
|
ab_frame_f32_t i_err; /* i_err = i_est - i_dq */
|
|
ab_frame_f32_t sign; /* Bang-bang controller sign */
|
|
};
|
|
|
|
/* FOC initialize data */
|
|
|
|
struct foc_initdata_f32_s
|
|
{
|
|
float id_kp; /* KP for d current */
|
|
float id_ki; /* KI for d current */
|
|
float iq_kp; /* KP for q current */
|
|
float iq_ki; /* KI for q current */
|
|
};
|
|
|
|
/* Field Oriented Control (FOC) data */
|
|
|
|
struct foc_data_f32_s
|
|
{
|
|
abc_frame_f32_t v_abc; /* Voltage in ABC frame */
|
|
ab_frame_f32_t v_ab; /* Voltage in alpha-beta frame */
|
|
dq_frame_f32_t v_dq; /* Requested voltage in dq frame */
|
|
ab_frame_f32_t v_ab_mod; /* Modulation voltage normalized to
|
|
* magnitude (0.0, 1.0)
|
|
*/
|
|
|
|
abc_frame_f32_t i_abc; /* Current in ABC frame */
|
|
ab_frame_f32_t i_ab; /* Current in alpha-beta frame */
|
|
dq_frame_f32_t i_dq; /* Current in dq frame */
|
|
dq_frame_f32_t i_dq_err; /* DQ current error */
|
|
|
|
dq_frame_f32_t i_dq_ref; /* Requested current for the FOC
|
|
* current controller
|
|
*/
|
|
|
|
pid_controller_f32_t id_pid; /* Current d-axis component PI controller */
|
|
pid_controller_f32_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 */
|
|
|
|
phase_angle_f32_t angle; /* Phase angle */
|
|
};
|
|
|
|
/* 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_f32_s
|
|
{
|
|
uint8_t p; /* Number of the motor pole pairs */
|
|
float res; /* Phase-to-neutral resistance */
|
|
float ind; /* Average phase-to-neutral inductance */
|
|
float one_by_ind; /* Inverse phase-to-neutral inductance */
|
|
};
|
|
|
|
/* PMSM motor physcial parameters */
|
|
|
|
struct pmsm_phy_params_f32_s
|
|
{
|
|
struct motor_phy_params_f32_s motor; /* Motor common PHY */
|
|
float iner; /* Rotor inertia */
|
|
float flux_link; /* Flux linkage */
|
|
float ind_d; /* d-inductance */
|
|
float ind_q; /* q-inductance */
|
|
float one_by_iner; /* One by intertia */
|
|
float one_by_indd; /* One by Ld */
|
|
float one_by_indq; /* One by Lq */
|
|
};
|
|
|
|
/* PMSM motor model state */
|
|
|
|
struct pmsm_model_state_f32_s
|
|
{
|
|
/* Motor model phase current */
|
|
|
|
abc_frame_f32_t i_abc;
|
|
ab_frame_f32_t i_ab;
|
|
dq_frame_f32_t i_dq;
|
|
|
|
/* Motor model phase voltage */
|
|
|
|
abc_frame_f32_t v_abc;
|
|
ab_frame_f32_t v_ab;
|
|
dq_frame_f32_t v_dq;
|
|
|
|
/* Motor model angle */
|
|
|
|
struct motor_angle_f32_s angle;
|
|
|
|
/* Angular speed */
|
|
|
|
float omega_e;
|
|
float omega_m;
|
|
};
|
|
|
|
/* PMSM motor model external conditions */
|
|
|
|
struct pmsm_model_ext_f32_s
|
|
{
|
|
float load; /* Motor model load torque */
|
|
};
|
|
|
|
/* PMSM motor model */
|
|
|
|
struct pmsm_model_f32_s
|
|
{
|
|
struct pmsm_phy_params_f32_s phy; /* Motor model physical parameters */
|
|
struct pmsm_model_state_f32_s state; /* Motor model state */
|
|
struct pmsm_model_ext_f32_s ext; /* Motor model external conditions */
|
|
float per; /* Control period */
|
|
float id_int; /* Id integral part */
|
|
float iq_int; /* Iq integral part */
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Public Functions Prototypes
|
|
****************************************************************************/
|
|
|
|
#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_f32_t *dq, float max);
|
|
float dq_mag(FAR dq_frame_f32_t *dq);
|
|
|
|
/* PID controller functions */
|
|
|
|
void pid_controller_init(FAR pid_controller_f32_t *pid,
|
|
float KP, float KI, float KD);
|
|
void pi_controller_init(FAR pid_controller_f32_t *pid,
|
|
float KP, float KI);
|
|
void pid_saturation_set(FAR pid_controller_f32_t *pid, float min, float max);
|
|
void pi_saturation_set(FAR pid_controller_f32_t *pid, float min, float max);
|
|
void pid_integral_reset(FAR pid_controller_f32_t *pid);
|
|
void pi_integral_reset(FAR pid_controller_f32_t *pid);
|
|
float pi_controller(FAR pid_controller_f32_t *pid, float err);
|
|
float pid_controller(FAR pid_controller_f32_t *pid, float err);
|
|
void pi_antiwindup_enable(FAR pid_controller_f32_t *pid, float KC,
|
|
bool enable);
|
|
void pi_ireset_enable(FAR pid_controller_f32_t *pid, bool enable);
|
|
|
|
/* Transformation functions */
|
|
|
|
void clarke_transform(FAR abc_frame_f32_t *abc, FAR ab_frame_f32_t *ab);
|
|
void inv_clarke_transform(FAR ab_frame_f32_t *ab, FAR abc_frame_f32_t *abc);
|
|
void park_transform(FAR phase_angle_f32_t *angle, FAR ab_frame_f32_t *ab,
|
|
FAR dq_frame_f32_t *dq);
|
|
void inv_park_transform(FAR phase_angle_f32_t *angle, FAR dq_frame_f32_t *dq,
|
|
FAR ab_frame_f32_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_f32_s *angle, float val);
|
|
|
|
/* 3-phase system space vector modulation */
|
|
|
|
void svm3_init(FAR struct svm3_state_f32_s *s);
|
|
void svm3(FAR struct svm3_state_f32_s *s, FAR ab_frame_f32_t *ab);
|
|
void svm3_current_correct(FAR struct svm3_state_f32_s *s,
|
|
float *c0, float *c1, float *c2);
|
|
|
|
/* Field Oriented Control */
|
|
|
|
void foc_init(FAR struct foc_data_f32_s *foc,
|
|
FAR struct foc_initdata_f32_s *init);
|
|
void foc_vbase_update(FAR struct foc_data_f32_s *foc, float vbase);
|
|
void foc_angle_update(FAR struct foc_data_f32_s *foc,
|
|
FAR phase_angle_f32_t *angle);
|
|
void foc_iabc_update(FAR struct foc_data_f32_s *foc,
|
|
FAR abc_frame_f32_t *i_abc);
|
|
void foc_voltage_control(FAR struct foc_data_f32_s *foc,
|
|
FAR dq_frame_f32_t *vdq_ref);
|
|
void foc_current_control(FAR struct foc_data_f32_s *foc,
|
|
FAR dq_frame_f32_t *idq_ref,
|
|
FAR dq_frame_f32_t *vdq_comp,
|
|
FAR dq_frame_f32_t *v_dq_ref);
|
|
void foc_vabmod_get(FAR struct foc_data_f32_s *foc,
|
|
FAR ab_frame_f32_t *v_ab_mod);
|
|
void foc_vdq_mag_max_get(FAR struct foc_data_f32_s *foc, FAR float *max);
|
|
|
|
/* BLDC/PMSM motor observers */
|
|
|
|
void motor_observer_init(FAR struct motor_observer_f32_s *observer,
|
|
FAR void *ao, FAR void *so, float per);
|
|
float motor_observer_speed_get(FAR struct motor_observer_f32_s *o);
|
|
float motor_observer_angle_get(FAR struct motor_observer_f32_s *o);
|
|
|
|
void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
|
|
float kslide, float err_max);
|
|
void motor_observer_smo(FAR struct motor_observer_f32_s *o,
|
|
FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
|
|
FAR struct motor_phy_params_f32_s *phy, float dir);
|
|
|
|
void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
|
|
uint8_t samples, float filer, float per);
|
|
void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
|
|
float angle, float dir);
|
|
|
|
/* Motor openloop control */
|
|
|
|
void motor_openloop_init(FAR struct openloop_data_f32_s *op, float per);
|
|
void motor_openloop(FAR struct openloop_data_f32_s *op, float speed,
|
|
float dir);
|
|
float motor_openloop_angle_get(FAR struct openloop_data_f32_s *op);
|
|
|
|
/* Motor angle */
|
|
|
|
void motor_angle_init(FAR struct motor_angle_f32_s *angle, uint8_t p);
|
|
void motor_angle_e_update(FAR struct motor_angle_f32_s *angle,
|
|
float angle_new, float dir);
|
|
void motor_angle_m_update(FAR struct motor_angle_f32_s *angle,
|
|
float angle_new, float dir);
|
|
float motor_angle_m_get(FAR struct motor_angle_f32_s *angle);
|
|
float motor_angle_e_get(FAR struct motor_angle_f32_s *angle);
|
|
|
|
/* Motor physical parameters */
|
|
|
|
void motor_phy_params_init(FAR struct motor_phy_params_f32_s *phy,
|
|
uint8_t poles, float res, float ind);
|
|
|
|
/* PMSM physical parameters functions */
|
|
|
|
void pmsm_phy_params_init(FAR struct pmsm_phy_params_f32_s *phy,
|
|
uint8_t poles, float res, float ind,
|
|
float iner, float flux,
|
|
float ind_d, float ind_q);
|
|
|
|
/* PMSM motor model */
|
|
|
|
int pmsm_model_initialize(FAR struct pmsm_model_f32_s *model,
|
|
FAR struct pmsm_phy_params_f32_s *phy,
|
|
float per);
|
|
int pmsm_model_elec(FAR struct pmsm_model_f32_s *model,
|
|
FAR ab_frame_f32_t *vab);
|
|
int pmsm_model_mech(FAR struct pmsm_model_f32_s *model, float load);
|
|
|
|
#undef EXTERN
|
|
#if defined(__cplusplus)
|
|
}
|
|
#endif
|
|
|
|
#endif /* __INCLUDE_DSP_H */
|