libdsp: add support for fixed16 libdsp
This commit is contained in:
raiden00pl
Xiang Xiao
parent
f248207717
commit
255ffcf852
@ -360,10 +360,7 @@ struct foc_data_f32_s
|
||||
struct motor_phy_params_f32_s
|
||||
{
|
||||
uint8_t p; /* Number of the motor pole pairs */
|
||||
|
||||
float res; /* Phase-to-neutral temperature compensated
|
||||
* resistance
|
||||
*/
|
||||
float res; /* Phase-to-neutral resistance */
|
||||
float ind; /* Average phase-to-neutral inductance */
|
||||
float one_by_ind; /* Inverse phase-to-neutral inductance */
|
||||
};
|
||||
|
||||
480
include/dspb16.h
Normal file
480
include/dspb16.h
Normal file
@ -0,0 +1,480 @@
|
||||
/****************************************************************************
|
||||
* include/dspb16.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_DSPB16_H
|
||||
#define __INCLUDE_DSPB16_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/compiler.h>
|
||||
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <fixedmath.h>
|
||||
|
||||
#include <assert.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_B16 ftob16(0.0f)
|
||||
#define DIR_CW_B16 ftob16(1.0f)
|
||||
#define DIR_CCW_B16 ftob16(-1.0f)
|
||||
|
||||
/* Some math constants ******************************************************/
|
||||
|
||||
#define SQRT3_BY_TWO_B16 ftob16(0.866025f)
|
||||
#define SQRT3_BY_THREE_B16 ftob16(0.57735f)
|
||||
#define ONE_BY_SQRT3_B16 ftob16(0.57735f)
|
||||
#define TWO_BY_SQRT3_B16 ftob16(1.15470f)
|
||||
|
||||
/* Some lib constants *******************************************************/
|
||||
|
||||
/* Motor electrical angle is in range 0.0 to 2*PI */
|
||||
|
||||
#define MOTOR_ANGLE_E_MAX_B16 (b16TWOPI)
|
||||
#define MOTOR_ANGLE_E_MIN_B16 (0)
|
||||
#define MOTOR_ANGLE_E_RANGE_B16 (MOTOR_ANGLE_E_MAX_B16 - MOTOR_ANGLE_E_MIN_B16)
|
||||
|
||||
/* Motor mechanical angle is in range 0.0 to 2*PI */
|
||||
|
||||
#define MOTOR_ANGLE_M_MAX_B16 (b16TWOPI)
|
||||
#define MOTOR_ANGLE_M_MIN_B16 (0)
|
||||
#define MOTOR_ANGLE_M_RANGE_B16 (MOTOR_ANGLE_M_MAX_B16 - MOTOR_ANGLE_M_MIN_B16)
|
||||
|
||||
/* Some useful macros *******************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: LP_FILTER_B16
|
||||
*
|
||||
* 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_B16(val, sample, filter) val -= (b16mulb16(filter, (val - sample)))
|
||||
|
||||
/****************************************************************************
|
||||
* Name: SVM3_BASE_VOLTAGE_GET_B16
|
||||
*
|
||||
* 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_B16(vbus) (b16mulb16(vbus, SQRT3_BY_THREE_B16))
|
||||
|
||||
/****************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************/
|
||||
|
||||
/* This structure represents phase angle.
|
||||
* Besides angle value it also stores sine and cosine values for given angle.
|
||||
*/
|
||||
|
||||
struct phase_angle_b16_s
|
||||
{
|
||||
b16_t angle; /* Phase angle in radians <0, 2PI> */
|
||||
b16_t sin; /* Phase angle sine */
|
||||
b16_t cos; /* Phase angle cosine */
|
||||
};
|
||||
|
||||
typedef struct phase_angle_b16_s phase_angle_b16_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_b16_s
|
||||
{
|
||||
phase_angle_b16_t angle_el; /* Electrical angle */
|
||||
b16_t anglem; /* Mechanical angle in radians <0, 2PI> */
|
||||
b16_t 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_b16_s
|
||||
{
|
||||
b16_t min; /* Lower limit */
|
||||
b16_t max; /* Upper limit */
|
||||
};
|
||||
|
||||
typedef struct float_sat_b16_s float_sat_b16_t;
|
||||
|
||||
/* PI/PID controller state structure */
|
||||
|
||||
struct pid_controller_b16_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 */
|
||||
b16_t out; /* Controller output */
|
||||
float_sat_b16_t sat; /* Output saturation */
|
||||
b16_t err; /* Current error value */
|
||||
b16_t err_prev; /* Previous error value */
|
||||
b16_t KP; /* Proportional coefficient */
|
||||
b16_t KI; /* Integral coefficient */
|
||||
b16_t KD; /* Derivative coefficient */
|
||||
b16_t part[3]; /* 0 - proporitonal part
|
||||
* 1 - integral part
|
||||
* 2 - derivative part
|
||||
*/
|
||||
b16_t KC; /* Integral anti-windup decay coefficient */
|
||||
b16_t aw; /* Integral anti-windup decay part */
|
||||
};
|
||||
|
||||
typedef struct pid_controller_b16_s pid_controller_b16_t;
|
||||
|
||||
/* This structure represents the ABC frame (3 phase vector) */
|
||||
|
||||
struct abc_frame_b16_s
|
||||
{
|
||||
b16_t a; /* A component */
|
||||
b16_t b; /* B component */
|
||||
b16_t c; /* C component */
|
||||
};
|
||||
|
||||
typedef struct abc_frame_b16_s abc_frame_b16_t;
|
||||
|
||||
/* This structure represents the alpha-beta frame (2 phase vector) */
|
||||
|
||||
struct ab_frame_b16_s
|
||||
{
|
||||
b16_t a; /* Alpha component */
|
||||
b16_t b; /* Beta component */
|
||||
};
|
||||
|
||||
typedef struct ab_frame_b16_s ab_frame_b16_t;
|
||||
|
||||
/* This structure represent the direct-quadrature frame */
|
||||
|
||||
struct dq_frame_b16_s
|
||||
{
|
||||
b16_t d; /* Driect component */
|
||||
b16_t q; /* Quadrature component */
|
||||
};
|
||||
|
||||
typedef struct dq_frame_b16_s dq_frame_b16_t;
|
||||
|
||||
/* Space Vector Modulation data for 3-phase system */
|
||||
|
||||
struct svm3_state_b16_s
|
||||
{
|
||||
uint8_t sector; /* Current space vector sector */
|
||||
b16_t d_u; /* Duty cycle for phase U */
|
||||
b16_t d_v; /* Duty cycle for phase V */
|
||||
b16_t d_w; /* Duty cycle for phase W */
|
||||
};
|
||||
|
||||
/* FOC initialize data */
|
||||
|
||||
struct foc_initdata_b16_s
|
||||
{
|
||||
b16_t id_kp; /* KP for d current */
|
||||
b16_t id_ki; /* KI for d current */
|
||||
b16_t iq_kp; /* KP for q current */
|
||||
b16_t iq_ki; /* KI for q current */
|
||||
};
|
||||
|
||||
/* Field Oriented Control (FOC) data */
|
||||
|
||||
struct foc_data_b16_s
|
||||
{
|
||||
abc_frame_b16_t v_abc; /* Voltage in ABC frame */
|
||||
ab_frame_b16_t v_ab; /* Voltage in alpha-beta frame */
|
||||
dq_frame_b16_t v_dq; /* Requested voltage in dq frame */
|
||||
ab_frame_b16_t v_ab_mod; /* Modulation voltage normalized to
|
||||
* magnitude (0.0, 1.0)
|
||||
*/
|
||||
|
||||
abc_frame_b16_t i_abc; /* Current in ABC frame */
|
||||
ab_frame_b16_t i_ab; /* Current in alpha-beta frame */
|
||||
dq_frame_b16_t i_dq; /* Current in dq frame */
|
||||
dq_frame_b16_t i_dq_err; /* DQ current error */
|
||||
|
||||
dq_frame_b16_t i_dq_ref; /* Requested current for the FOC
|
||||
* current controler
|
||||
*/
|
||||
|
||||
pid_controller_b16_t id_pid; /* Current d-axis component PI controller */
|
||||
pid_controller_b16_t iq_pid; /* Current q-axis component PI controller */
|
||||
|
||||
b16_t vdq_mag_max; /* Maximum dq voltage magnitude */
|
||||
b16_t vab_mod_scale; /* Voltage alpha-beta modulation scale */
|
||||
|
||||
phase_angle_b16_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_b16_s
|
||||
{
|
||||
uint8_t p; /* Number of the motor pole pairs */
|
||||
b16_t res; /* Phase-to-neutral temperature compensated
|
||||
* resistance
|
||||
*/
|
||||
b16_t ind; /* Average phase-to-neutral inductance */
|
||||
b16_t one_by_ind; /* Inverse phase-to-neutral inductance */
|
||||
};
|
||||
|
||||
/* PMSM motor physcial parameters */
|
||||
|
||||
struct pmsm_phy_params_b16_s
|
||||
{
|
||||
struct motor_phy_params_b16_s motor; /* Motor common PHY */
|
||||
b16_t iner; /* Rotor inertia */
|
||||
b16_t flux_link; /* Flux linkage */
|
||||
b16_t ind_d; /* d-inductance */
|
||||
b16_t ind_q; /* q-inductance */
|
||||
b16_t one_by_iner; /* One by J */
|
||||
b16_t one_by_indd; /* One by Ld */
|
||||
b16_t one_by_indq; /* One by Lq */
|
||||
};
|
||||
|
||||
/* PMSM motor model state */
|
||||
|
||||
struct pmsm_model_state_b16_s
|
||||
{
|
||||
/* Motor model phase current */
|
||||
|
||||
abc_frame_b16_t i_abc;
|
||||
ab_frame_b16_t i_ab;
|
||||
dq_frame_b16_t i_dq;
|
||||
|
||||
/* Motor model phase voltage */
|
||||
|
||||
abc_frame_b16_t v_abc;
|
||||
ab_frame_b16_t v_ab;
|
||||
dq_frame_b16_t v_dq;
|
||||
|
||||
/* Motor model angle */
|
||||
|
||||
struct motor_angle_b16_s angle;
|
||||
|
||||
/* Angular speed */
|
||||
|
||||
b16_t omega_e;
|
||||
b16_t omega_m;
|
||||
};
|
||||
|
||||
/* PMSM motor model external conditions */
|
||||
|
||||
struct pmsm_model_ext_b16_s
|
||||
{
|
||||
b16_t load; /* Motor model load torque */
|
||||
};
|
||||
|
||||
/* PMSM motor model */
|
||||
|
||||
struct pmsm_model_b16_s
|
||||
{
|
||||
struct pmsm_phy_params_b16_s phy; /* Motor model physical parameters */
|
||||
struct pmsm_model_state_b16_s state; /* Motor model state */
|
||||
struct pmsm_model_ext_b16_s ext; /* Motor model external conditions */
|
||||
b16_t per; /* Control period */
|
||||
b16_t id_int; /* Id integral part */
|
||||
b16_t 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 */
|
||||
|
||||
b16_t fast_sin_b16(b16_t angle);
|
||||
b16_t fast_sin2_b16(b16_t angle);
|
||||
b16_t fast_cos_b16(b16_t angle);
|
||||
b16_t fast_cos2_b16(b16_t angle);
|
||||
b16_t fast_atan2_b16(b16_t y, b16_t x);
|
||||
void f_saturate_b16(FAR b16_t *val, b16_t min, b16_t max);
|
||||
b16_t vector2d_mag_b16(b16_t x, b16_t y);
|
||||
void vector2d_saturate_b16(FAR b16_t *x, FAR b16_t *y, b16_t max);
|
||||
void dq_saturate_b16(FAR dq_frame_b16_t *dq, b16_t max);
|
||||
b16_t dq_mag_b16(FAR dq_frame_b16_t *dq);
|
||||
|
||||
/* PID controller functions */
|
||||
|
||||
void pid_controller_init_b16(FAR pid_controller_b16_t *pid,
|
||||
b16_t KP, b16_t KI, b16_t KD);
|
||||
void pi_controller_init_b16(FAR pid_controller_b16_t *pid,
|
||||
b16_t KP, b16_t KI);
|
||||
void pid_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min,
|
||||
b16_t max);
|
||||
void pi_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min,
|
||||
b16_t max);
|
||||
void pid_integral_reset_b16(FAR pid_controller_b16_t *pid);
|
||||
void pi_integral_reset_b16(FAR pid_controller_b16_t *pid);
|
||||
b16_t pi_controller_b16(FAR pid_controller_b16_t *pid, b16_t err);
|
||||
b16_t pid_controller_b16(FAR pid_controller_b16_t *pid, b16_t err);
|
||||
void pi_antiwindup_enable_b16(FAR pid_controller_b16_t *pid, b16_t KC,
|
||||
bool enable);
|
||||
void pi_ireset_enable_b16(FAR pid_controller_b16_t *pid, bool enable);
|
||||
|
||||
/* Transformation functions */
|
||||
|
||||
void clarke_transform_b16(FAR abc_frame_b16_t *abc, FAR ab_frame_b16_t *ab);
|
||||
void inv_clarke_transform_b16(FAR ab_frame_b16_t *ab,
|
||||
FAR abc_frame_b16_t *abc);
|
||||
void park_transform_b16(FAR phase_angle_b16_t *angle, FAR ab_frame_b16_t *ab,
|
||||
FAR dq_frame_b16_t *dq);
|
||||
void inv_park_transform_b16(FAR phase_angle_b16_t *angle,
|
||||
FAR dq_frame_b16_t *dq, FAR ab_frame_b16_t *ab);
|
||||
|
||||
/* Phase angle related functions */
|
||||
|
||||
void angle_norm_b16(FAR b16_t *angle, b16_t per, b16_t bottom, b16_t top);
|
||||
void angle_norm_2pi_b16(FAR b16_t *angle, b16_t bottom, b16_t top);
|
||||
void phase_angle_update_b16(FAR struct phase_angle_b16_s *angle, b16_t val);
|
||||
|
||||
/* 3-phase system space vector modulation */
|
||||
|
||||
void svm3_init_b16(FAR struct svm3_state_b16_s *s);
|
||||
void svm3_b16(FAR struct svm3_state_b16_s *s, FAR ab_frame_b16_t *ab);
|
||||
void svm3_current_correct_b16(FAR struct svm3_state_b16_s *s,
|
||||
b16_t *c0, b16_t *c1, b16_t *c2);
|
||||
|
||||
/* Field Oriented Control */
|
||||
|
||||
void foc_init_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR struct foc_initdata_b16_s *init);
|
||||
void foc_vbase_update_b16(FAR struct foc_data_b16_s *foc, b16_t vbase);
|
||||
void foc_angle_update_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR phase_angle_b16_t *angle);
|
||||
void foc_iabc_update_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR abc_frame_b16_t *i_abc);
|
||||
void foc_voltage_control_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *vdq_ref);
|
||||
void foc_current_control_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *idq_ref,
|
||||
FAR dq_frame_b16_t *vdq_comp,
|
||||
FAR dq_frame_b16_t *v_dq_ref);
|
||||
void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR ab_frame_b16_t *v_ab_mod);
|
||||
void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max);
|
||||
|
||||
/* Motor angle */
|
||||
|
||||
void motor_angle_init_b16(FAR struct motor_angle_b16_s *angle, uint8_t p);
|
||||
void motor_angle_e_update_b16(FAR struct motor_angle_b16_s *angle,
|
||||
b16_t angle_new, b16_t dir);
|
||||
void motor_angle_m_update_b16(FAR struct motor_angle_b16_s *angle,
|
||||
b16_t angle_new, b16_t dir);
|
||||
b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle);
|
||||
b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle);
|
||||
|
||||
/* Motor physical parameters */
|
||||
|
||||
void motor_phy_params_init_b16(FAR struct motor_phy_params_b16_s *phy,
|
||||
uint8_t poles, b16_t res, b16_t ind);
|
||||
|
||||
/* PMSM physical parameters functions */
|
||||
|
||||
void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy,
|
||||
uint8_t poles, b16_t res, b16_t ind,
|
||||
b16_t iner, b16_t flux,
|
||||
b16_t ind_d, b16_t ind_q);
|
||||
|
||||
/* PMSM motor model */
|
||||
|
||||
int pmsm_model_initialize_b16(FAR struct pmsm_model_b16_s *model,
|
||||
FAR struct pmsm_phy_params_b16_s *phy,
|
||||
b16_t per);
|
||||
int pmsm_model_elec_b16(FAR struct pmsm_model_b16_s *model,
|
||||
FAR ab_frame_b16_t *vab);
|
||||
int pmsm_model_mech_b16(FAR struct pmsm_model_b16_s *model, b16_t load);
|
||||
|
||||
#undef EXTERN
|
||||
#if defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __INCLUDE_DSPB16_H */
|
||||
@ -29,6 +29,14 @@ CSRCS += lib_foc.c
|
||||
CSRCS += lib_misc.c
|
||||
CSRCS += lib_motor.c
|
||||
CSRCS += lib_pmsm_model.c
|
||||
|
||||
CSRCS += lib_pid_b16.c
|
||||
CSRCS += lib_svm_b16.c
|
||||
CSRCS += lib_transform_b16.c
|
||||
CSRCS += lib_foc_b16.c
|
||||
CSRCS += lib_misc_b16.c
|
||||
CSRCS += lib_motor_b16.c
|
||||
CSRCS += lib_pmsm_model_b16.c
|
||||
endif
|
||||
|
||||
AOBJS = $(ASRCS:.S=$(OBJEXT))
|
||||
|
||||
464
libs/libdsp/lib_foc_b16.c
Normal file
464
libs/libdsp/lib_foc_b16.c
Normal file
@ -0,0 +1,464 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_foc_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_current_controler_b16
|
||||
*
|
||||
* Description:
|
||||
* This function implements FOC current controler algorithm.
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* v_dq_req - (in) pointer to the voltage DQ request frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void foc_current_controler_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *v_dq_req)
|
||||
{
|
||||
FAR pid_controller_b16_t *id_pid = &foc->id_pid;
|
||||
FAR pid_controller_b16_t *iq_pid = &foc->iq_pid;
|
||||
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(v_dq_req != NULL);
|
||||
|
||||
/* Get dq current error */
|
||||
|
||||
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;
|
||||
|
||||
/* NOTE: PI controllers saturation is updated in foc_vdq_mag_max_set() */
|
||||
|
||||
/* PI controller for d-current (flux loop) */
|
||||
|
||||
v_dq_req->d = pi_controller_b16(id_pid, foc->i_dq_err.d);
|
||||
|
||||
/* PI controller for q-current (torque loop) */
|
||||
|
||||
v_dq_req->q = pi_controller_b16(iq_pid, foc->i_dq_err.q);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vab_mod_scale_set_b16
|
||||
*
|
||||
* 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_b16(FAR struct foc_data_b16_s *foc,
|
||||
b16_t scale)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
|
||||
foc->vab_mod_scale = scale;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vdq_mag_max_set_b16
|
||||
*
|
||||
* 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_b16(FAR struct foc_data_b16_s *foc,
|
||||
b16_t max)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
|
||||
foc->vdq_mag_max = max;
|
||||
|
||||
/* Update regulators saturation */
|
||||
|
||||
pi_saturation_set_b16(&foc->id_pid, -foc->vdq_mag_max, foc->vdq_mag_max);
|
||||
pi_saturation_set_b16(&foc->iq_pid, -foc->vdq_mag_max, foc->vdq_mag_max);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vdq_ref_set_b16
|
||||
*
|
||||
* Description:
|
||||
* Set dq requested voltage vector
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* vdq_ref - (in) pointer to the requested idq voltage
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void foc_vdq_ref_set_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *vdq_ref)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(vdq_ref != NULL);
|
||||
|
||||
foc->v_dq.d = vdq_ref->d;
|
||||
foc->v_dq.q = vdq_ref->q;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_idq_ref_set_b16
|
||||
*
|
||||
* Description:
|
||||
* Set dq reference current vector
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* idq_ref - (in) pointer to the reference idq current
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void foc_idq_ref_set_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *idq_ref)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(idq_ref != NULL);
|
||||
|
||||
foc->i_dq_ref.d = idq_ref->d;
|
||||
foc->i_dq_ref.q = idq_ref->q;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_init_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize FOC controller
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* init - (in) pointer to the FOC initialization data
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_init_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR struct foc_initdata_b16_s *init)
|
||||
{
|
||||
/* Reset data */
|
||||
|
||||
memset(foc, 0, sizeof(struct foc_data_b16_s));
|
||||
|
||||
/* Initialize PI current d component */
|
||||
|
||||
pi_controller_init_b16(&foc->id_pid, init->id_kp, init->id_ki);
|
||||
|
||||
/* Initialize PI current q component */
|
||||
|
||||
pi_controller_init_b16(&foc->iq_pid, init->iq_kp, init->iq_ki);
|
||||
|
||||
/* Disable PI intergral part reset when saturated */
|
||||
|
||||
pi_ireset_enable_b16(&foc->iq_pid, false);
|
||||
pi_ireset_enable_b16(&foc->id_pid, false);
|
||||
|
||||
/* Enable PI anti-windup protection */
|
||||
|
||||
pi_antiwindup_enable_b16(&foc->iq_pid, ftob16(0.99f), true);
|
||||
pi_antiwindup_enable_b16(&foc->id_pid, ftob16(0.99f), true);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vbase_update_b16
|
||||
*
|
||||
* 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_b16(FAR struct foc_data_b16_s *foc, b16_t vbase)
|
||||
{
|
||||
b16_t scale = 0;
|
||||
b16_t mag_max = 0;
|
||||
|
||||
/* Prevent division by zero */
|
||||
|
||||
if (vbase < 1)
|
||||
{
|
||||
vbase = 1;
|
||||
}
|
||||
|
||||
/* NOTE: this is base voltage for FOC, not bus voltage! */
|
||||
|
||||
scale = b16divb16(b16ONE, vbase);
|
||||
mag_max = vbase;
|
||||
|
||||
/* Update */
|
||||
|
||||
foc_vab_mod_scale_set_b16(foc, scale);
|
||||
foc_vdq_mag_max_set_b16(foc, mag_max);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_angle_update_b16
|
||||
*
|
||||
* Description:
|
||||
* Update FOC data with new motor phase angle.
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* angle - (in) pointer to the phase angle data
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_angle_update_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR phase_angle_b16_t *angle)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
|
||||
/* Copy angle to foc data */
|
||||
|
||||
foc->angle.angle = angle->angle;
|
||||
foc->angle.sin = angle->sin;
|
||||
foc->angle.cos = angle->cos;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_iabc_update_b16
|
||||
*
|
||||
* Description:
|
||||
* Update FOC data with new iabc frame.
|
||||
*
|
||||
* To work properly this function requires some additional steps:
|
||||
* 1. phase angle must be set with foc_angle_update()
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* i_abc - (in) pointer to the ABC current frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_iabc_update_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR abc_frame_b16_t *i_abc)
|
||||
{
|
||||
dq_frame_b16_t i_dq;
|
||||
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(i_abc != NULL);
|
||||
|
||||
/* Reset data */
|
||||
|
||||
i_dq.d = 0;
|
||||
i_dq.q = 0;
|
||||
|
||||
/* 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_b16(&foc->i_abc, &foc->i_ab);
|
||||
|
||||
/* Convert alpha-beta current to dq current */
|
||||
|
||||
park_transform_b16(&foc->angle, &foc->i_ab, &i_dq);
|
||||
|
||||
/* Store dq current */
|
||||
|
||||
foc->i_dq.d = i_dq.d;
|
||||
foc->i_dq.q = i_dq.q;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_voltage_control_b16
|
||||
*
|
||||
* Description:
|
||||
* Process FOC voltage control.
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* vdq_ref - (in) voltage dq reference frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_voltage_control_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *vdq_ref)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
|
||||
/* Update VDQ request */
|
||||
|
||||
foc_vdq_ref_set_b16(foc, vdq_ref);
|
||||
|
||||
/* Inverse Park transform (voltage dq -> voltage alpha-beta) */
|
||||
|
||||
inv_park_transform_b16(&foc->angle, &foc->v_dq, &foc->v_ab);
|
||||
|
||||
#ifdef CONFIG_LIBDSP_FOC_VABC
|
||||
/* Inverse Clarke transform (voltage alpha-beta -> voltage abc) */
|
||||
|
||||
inv_clarke_transform_b16(&foc->v_ab, &foc->v_abc);
|
||||
#endif
|
||||
|
||||
/* Normalize the alpha-beta voltage to get the alpha-beta modulation
|
||||
* voltage
|
||||
*/
|
||||
|
||||
foc->v_ab_mod.a = b16mulb16(foc->v_ab.a, foc->vab_mod_scale);
|
||||
foc->v_ab_mod.b = b16mulb16(foc->v_ab.b, foc->vab_mod_scale);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_current_control_b16
|
||||
*
|
||||
* Description:
|
||||
* Process FOC current control.
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* idq_ref - (in) current dq reference frame
|
||||
* vdq_comp - (in) voltage dq compensation frame
|
||||
* vdq_ref - (out) voltage dq reference frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
* TODO: add some reference and a brief description of the FOC
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_current_control_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR dq_frame_b16_t *idq_ref,
|
||||
FAR dq_frame_b16_t *vdq_comp,
|
||||
FAR dq_frame_b16_t *vdq_ref)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
|
||||
/* Update IDQ reference */
|
||||
|
||||
foc_idq_ref_set_b16(foc, idq_ref);
|
||||
|
||||
/* Run FOC current controler (current dq -> voltage dq) */
|
||||
|
||||
foc_current_controler_b16(foc, vdq_ref);
|
||||
|
||||
/* DQ voltage compensation */
|
||||
|
||||
vdq_ref->d = vdq_ref->d - vdq_comp->d;
|
||||
vdq_ref->q = vdq_ref->q - vdq_comp->q;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vabmod_get_b16
|
||||
*
|
||||
* Description:
|
||||
* Get result from the FOC controler (foc_current_control or
|
||||
* foc_voltage_control)
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* v_ab_mod - (out) pointer to the voltage alpha-beta modulation frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc,
|
||||
FAR ab_frame_b16_t *v_ab_mod)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
LIBDSP_DEBUGASSERT(v_ab_mod != NULL);
|
||||
|
||||
v_ab_mod->a = foc->v_ab_mod.a;
|
||||
v_ab_mod->b = foc->v_ab_mod.b;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: foc_vdq_mag_max_get_b16
|
||||
*
|
||||
* Description:
|
||||
* Get maximum dq voltage vector magnitude
|
||||
*
|
||||
* Input Parameters:
|
||||
* foc - (in/out) pointer to the FOC data
|
||||
* max - (out) maximum dq voltage magnitude
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(foc != NULL);
|
||||
|
||||
*max = foc->vdq_mag_max;
|
||||
}
|
||||
457
libs/libdsp/lib_misc_b16.c
Normal file
457
libs/libdsp/lib_misc_b16.c
Normal file
@ -0,0 +1,457 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_misc_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define VECTOR2D_SATURATE_MAG_MIN (1)
|
||||
#define FAST_ATAN2_SMALLNUM (1)
|
||||
|
||||
#ifndef ABS
|
||||
# define ABS(a) (a < 0 ? -a : a)
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: f_saturate_b16
|
||||
*
|
||||
* Description:
|
||||
* Saturate b16_t number
|
||||
*
|
||||
* Input Parameters:
|
||||
* val - pointer to b16_t number
|
||||
* min - lower limit
|
||||
* max - upper limit
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void f_saturate_b16(FAR b16_t *val, b16_t min, b16_t max)
|
||||
{
|
||||
if (*val < min)
|
||||
{
|
||||
*val = min;
|
||||
}
|
||||
|
||||
else if (*val > max)
|
||||
{
|
||||
*val = max;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: vector2d_mag_b16
|
||||
*
|
||||
* Description:
|
||||
* Get 2D vector magnitude.
|
||||
*
|
||||
* Input Parameters:
|
||||
* x - (in) vector x component
|
||||
* y - (in) vector y component
|
||||
*
|
||||
* Returned Value:
|
||||
* Return 2D vector magnitude
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t vector2d_mag_b16(b16_t x, b16_t y)
|
||||
{
|
||||
b16_t t0 = 0;
|
||||
b16_t t1 = 0;
|
||||
b16_t t3 = 0;
|
||||
|
||||
t0 = b16sqr(x);
|
||||
t1 = b16sqr(y);
|
||||
t3 = t0 + t1;
|
||||
|
||||
/* TODO: move to fixedmath sqrt */
|
||||
|
||||
if (t3 == 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
#if CONFIG_LIBDSP_PRECISION == 0
|
||||
/* Use ub8 sqrt */
|
||||
|
||||
return ub8toub16(ub16sqrtub8(t3));
|
||||
#else
|
||||
/* Too slow ! */
|
||||
|
||||
return ub16sqrtub16(t3);
|
||||
#endif
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: vector2d_saturate_b16
|
||||
*
|
||||
* Description:
|
||||
* Saturate 2D vector magnitude.
|
||||
*
|
||||
* Input Parameters:
|
||||
* x - (in/out) pointer to the vector x component
|
||||
* y - (in/out) pointer to the vector y component
|
||||
* max - (in) maximum vector magnitude
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void vector2d_saturate_b16(FAR b16_t *x, FAR b16_t *y, b16_t max)
|
||||
{
|
||||
b16_t mag = 0;
|
||||
b16_t tmp = 0;
|
||||
|
||||
/* Get vector magnitude */
|
||||
|
||||
mag = vector2d_mag_b16(*x, *y);
|
||||
|
||||
if (mag < VECTOR2D_SATURATE_MAG_MIN)
|
||||
{
|
||||
mag = VECTOR2D_SATURATE_MAG_MIN;
|
||||
}
|
||||
|
||||
if (mag > max)
|
||||
{
|
||||
/* Saturate vector */
|
||||
|
||||
tmp = b16divb16(max, mag);
|
||||
*x = b16mulb16(*x, tmp);
|
||||
*y = b16mulb16(*x, tmp);
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: dq_mag_b16
|
||||
*
|
||||
* Description:
|
||||
* Get DQ vector magnitude.
|
||||
*
|
||||
* Input Parameters:
|
||||
* dq - (in/out) dq frame vector
|
||||
*
|
||||
* Returned Value:
|
||||
* Return dq vector magnitude
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t dq_mag_b16(FAR dq_frame_b16_t *dq)
|
||||
{
|
||||
return vector2d_mag_b16(dq->d, dq->q);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: dq_saturate_b16
|
||||
*
|
||||
* Description:
|
||||
* Saturate dq frame vector magnitude.
|
||||
*
|
||||
* Input Parameters:
|
||||
* dq - (in/out) dq frame vector
|
||||
* max - (in) maximum vector magnitude
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void dq_saturate_b16(FAR dq_frame_b16_t *dq, b16_t max)
|
||||
{
|
||||
vector2d_saturate_b16(&dq->d, &dq->q, max);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: fast_sin_b16
|
||||
*
|
||||
* Description:
|
||||
* Fast sin calculation
|
||||
*
|
||||
* Reference: http://lab.polygonal.de/?p=205
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in)
|
||||
*
|
||||
* Returned Value:
|
||||
* Return estimated sine value
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t fast_sin_b16(b16_t angle)
|
||||
{
|
||||
b16_t sin = 0;
|
||||
b16_t n1 = ftob16(1.27323954f);
|
||||
b16_t n2 = ftob16(0.405284735f);
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
|
||||
/* Normalize angle */
|
||||
|
||||
angle_norm_2pi_b16(&angle, -b16PI, b16PI);
|
||||
|
||||
/* Get estiamte sine value from quadratic equation */
|
||||
|
||||
if (angle < 0)
|
||||
{
|
||||
tmp1 = b16mulb16(n1, angle);
|
||||
tmp2 = b16mulb16(n2, b16sqr(angle));
|
||||
sin = tmp1 + tmp2;
|
||||
}
|
||||
else
|
||||
{
|
||||
tmp1 = b16mulb16(n1, angle);
|
||||
tmp2 = b16mulb16(n2, b16sqr(angle));
|
||||
sin = tmp1 - tmp2;
|
||||
}
|
||||
|
||||
return sin;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name:fast_cos_b16
|
||||
*
|
||||
* Description:
|
||||
* Fast cos calculation
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in)
|
||||
*
|
||||
* Returned Value:
|
||||
* Return estimated cosine value
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t fast_cos_b16(b16_t angle)
|
||||
{
|
||||
/* Get cosine value from sine sin(x + PI/2) = cos(x) */
|
||||
|
||||
return fast_sin_b16(angle + b16HALFPI);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: fast_sin2_b16
|
||||
*
|
||||
* Description:
|
||||
* Fast sin calculation with better accuracy
|
||||
*
|
||||
* Reference: http://lab.polygonal.de/?p=205
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle
|
||||
*
|
||||
* Returned Value:
|
||||
* Return estimated sine value
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t fast_sin2_b16(b16_t angle)
|
||||
{
|
||||
return b16sin(angle);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name:fast_cos2_b16
|
||||
*
|
||||
* Description:
|
||||
* Fast cos calculation with better accuracy
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in)
|
||||
*
|
||||
* Returned Value:
|
||||
* Return estimated cosine value
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t fast_cos2_b16(b16_t angle)
|
||||
{
|
||||
return b16cos(angle);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: fast_atan2_b16
|
||||
*
|
||||
* Description:
|
||||
* Fast atan2 calculation
|
||||
*
|
||||
* REFERENCE:
|
||||
* https://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization/
|
||||
*
|
||||
* Input Parameters:
|
||||
* x - (in)
|
||||
* y - (in)
|
||||
*
|
||||
* Returned Value:
|
||||
* Return estimated angle
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t fast_atan2_b16(b16_t y, b16_t x)
|
||||
{
|
||||
b16_t angle = 0;
|
||||
b16_t abs_y = 0;
|
||||
b16_t rsq = 0;
|
||||
b16_t r = 0;
|
||||
b16_t n1 = ftob16(0.1963f);
|
||||
b16_t n2 = ftob16(0.9817f);
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
b16_t tmp3 = 0;
|
||||
|
||||
/* Get absolute value of y and add some small number to prevent 0/0 */
|
||||
|
||||
abs_y = ABS(y) + FAST_ATAN2_SMALLNUM;
|
||||
|
||||
/* Calculate angle */
|
||||
|
||||
if (x >= 0)
|
||||
{
|
||||
r = b16divb16((x - abs_y), (x + abs_y));
|
||||
rsq = b16mulb16(r, r);
|
||||
tmp1 = b16mulb16(n1, rsq);
|
||||
tmp2 = b16mulb16((tmp1 - n2), r);
|
||||
tmp3 = b16mulb16(b16PI, ftob16(0.25f));
|
||||
angle = tmp2 + tmp3;
|
||||
}
|
||||
else
|
||||
{
|
||||
r = b16divb16((x + abs_y), (abs_y - x));
|
||||
rsq = b16mulb16(r, r);
|
||||
tmp1 = b16mulb16(n1, rsq);
|
||||
tmp2 = b16mulb16((tmp1 - n2), r);
|
||||
tmp3 = b16mulb16(b16PI, ftob16(0.75f));
|
||||
angle = tmp2 + tmp3;
|
||||
}
|
||||
|
||||
/* Get angle sign */
|
||||
|
||||
if (y < 0)
|
||||
{
|
||||
angle = -angle;
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: angle_norm_b16
|
||||
*
|
||||
* Description:
|
||||
* Normalize radians angle to a given boundary and a given period.
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in/out) pointer to the angle data
|
||||
* per - (in) angle period
|
||||
* bottom - (in) lower limit
|
||||
* top - (in) upper limit
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void angle_norm_b16(FAR b16_t *angle, b16_t per, b16_t bottom, b16_t top)
|
||||
{
|
||||
while (*angle > top)
|
||||
{
|
||||
/* Move the angle backwards by given period */
|
||||
|
||||
*angle = *angle - per;
|
||||
}
|
||||
|
||||
while (*angle < bottom)
|
||||
{
|
||||
/* Move the angle forwards by given period */
|
||||
|
||||
*angle = *angle + per;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: angle_norm_2pi_b16
|
||||
*
|
||||
* Description:
|
||||
* Normalize radians angle with period 2*PI to a given boundary.
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in/out) pointer to the angle data
|
||||
* bottom - (in) lower limit
|
||||
* top - (in) upper limit
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void angle_norm_2pi_b16(FAR b16_t *angle, b16_t bottom, b16_t top)
|
||||
{
|
||||
angle_norm_b16(angle, b16TWOPI, bottom, top);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: phase_angle_update_b16
|
||||
*
|
||||
* Description:
|
||||
* Update phase_angle_s structure:
|
||||
* 1. normalize angle value to <0.0, 2PI> range
|
||||
* 2. update angle value
|
||||
* 3. update sin/cos value for given angle
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in/out) pointer to the angle data
|
||||
* val - (in) angle radian value
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void phase_angle_update_b16(FAR struct phase_angle_b16_s *angle, b16_t val)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
|
||||
/* Normalize angle to <0.0, 2PI> */
|
||||
|
||||
angle_norm_2pi_b16(&val, 0, b16TWOPI);
|
||||
|
||||
/* Update structure */
|
||||
|
||||
angle->angle = val;
|
||||
|
||||
#if CONFIG_LIBDSP_PRECISION == 0
|
||||
angle->sin = fast_sin_b16(val);
|
||||
angle->cos = fast_cos_b16(val);
|
||||
#else
|
||||
angle->sin = fast_sin2_b16(val);
|
||||
angle->cos = fast_cos2_b16(val);
|
||||
#endif
|
||||
}
|
||||
279
libs/libdsp/lib_motor_b16.c
Normal file
279
libs/libdsp/lib_motor_b16.c
Normal file
@ -0,0 +1,279 @@
|
||||
/****************************************************************************
|
||||
* control/lib_motor_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define POLE_CNTR_THR (0)
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: motor_angle_e_update_b16
|
||||
*
|
||||
* 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_b16(FAR struct motor_angle_b16_s *angle,
|
||||
b16_t angle_new, b16_t dir)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
|
||||
LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
|
||||
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
|
||||
/* Check if we crossed electrical angle boundaries */
|
||||
|
||||
if (dir == DIR_CW_B16)
|
||||
{
|
||||
/* For CW direction - previous angle is greater than current angle */
|
||||
|
||||
if (angle_new - angle->angle_el.angle < -POLE_CNTR_THR)
|
||||
{
|
||||
angle->i += 1;
|
||||
}
|
||||
}
|
||||
|
||||
else if (dir == DIR_CCW_B16)
|
||||
{
|
||||
/* For CCW direction - previous angle is lower than current angle */
|
||||
|
||||
if (angle_new - angle->angle_el.angle > POLE_CNTR_THR)
|
||||
{
|
||||
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_b16(&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.
|
||||
*/
|
||||
|
||||
tmp1 = b16mulb16(MOTOR_ANGLE_E_RANGE_B16, itob16(angle->i));
|
||||
tmp2 = tmp1 + angle->angle_el.angle;
|
||||
|
||||
angle->anglem = b16mulb16(tmp2, angle->one_by_p);
|
||||
|
||||
/* Normalize mechanical angle to <0, 2PI> and store */
|
||||
|
||||
angle_norm_2pi_b16(&angle->anglem, MOTOR_ANGLE_M_MIN_B16,
|
||||
MOTOR_ANGLE_M_MAX_B16);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: motor_angle_m_update_b16
|
||||
*
|
||||
* 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_b16(FAR struct motor_angle_b16_s *angle,
|
||||
b16_t angle_new, b16_t dir)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
LIBDSP_DEBUGASSERT(angle_new >= 0 && angle_new <= MOTOR_ANGLE_E_MAX_B16);
|
||||
LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
|
||||
|
||||
b16_t angle_el = 0;
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
|
||||
/* Store new mechanical angle */
|
||||
|
||||
angle->anglem = angle_new;
|
||||
|
||||
/* Update pole counter */
|
||||
|
||||
tmp1 = b16mulb16(angle->anglem, itob16(angle->p));
|
||||
|
||||
angle->i = (uint8_t) b16toi(b16divb16(tmp1, MOTOR_ANGLE_M_MAX_B16));
|
||||
|
||||
/* Get electrical angle */
|
||||
|
||||
tmp1 = b16mulb16(angle->anglem, itob16(angle->p));
|
||||
tmp2 = b16mulb16(MOTOR_ANGLE_E_MAX_B16, itob16(angle->i));
|
||||
|
||||
angle_el = (tmp1 - tmp2);
|
||||
|
||||
/* Update electrical angle structure */
|
||||
|
||||
phase_angle_update_b16(&angle->angle_el, angle_el);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: motor_angle_m_get_b16
|
||||
*
|
||||
* Description:
|
||||
* Get motor mechanical angle
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in/out) pointer to the motor angle structure
|
||||
*
|
||||
* Returned Value:
|
||||
* Return motor mechanical angle
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t motor_angle_m_get_b16(FAR struct motor_angle_b16_s *angle)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
|
||||
return angle->anglem;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: motor_angle_e_get_b16
|
||||
*
|
||||
* Description:
|
||||
* Get motor electrical angle
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in/out) pointer to the motor angle structure
|
||||
*
|
||||
* Returned Value:
|
||||
* Return motor electrical angle
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t motor_angle_e_get_b16(FAR struct motor_angle_b16_s *angle)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
|
||||
return angle->angle_el.angle;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: motor_phy_params_init_b16
|
||||
*
|
||||
* 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_b16(FAR struct motor_phy_params_b16_s *phy,
|
||||
uint8_t poles, b16_t res, b16_t ind)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(phy != NULL);
|
||||
|
||||
memset(phy, 0, sizeof(struct motor_phy_params_b16_s));
|
||||
|
||||
phy->p = poles;
|
||||
phy->res = res;
|
||||
phy->ind = ind;
|
||||
phy->one_by_ind = b16divb16(b16ONE, ind);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pmsm_phy_params_init_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize PMSM physical parameters
|
||||
*
|
||||
* Input Parameters:
|
||||
* phy - (in/out) pointer to the PMSM 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
|
||||
* iner - (in) rotor inertia (J)
|
||||
* flux - (in) flux linkage
|
||||
* ind_d - (in) d-inductance
|
||||
* ind_q - (in) q-inductance
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void pmsm_phy_params_init_b16(FAR struct pmsm_phy_params_b16_s *phy,
|
||||
uint8_t poles, b16_t res, b16_t ind,
|
||||
b16_t iner, b16_t flux,
|
||||
b16_t ind_d, b16_t ind_q)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(phy != NULL);
|
||||
|
||||
/* Initialize motor phy */
|
||||
|
||||
motor_phy_params_init_b16(&phy->motor, poles, res, ind);
|
||||
|
||||
/* Iniitalize PMSM specific data */
|
||||
|
||||
phy->iner = iner;
|
||||
phy->flux_link = flux;
|
||||
phy->ind_d = ind_d;
|
||||
phy->ind_q = ind_q;
|
||||
phy->one_by_iner = b16divb16(b16ONE, iner);
|
||||
phy->one_by_indd = b16divb16(b16ONE, ind_d);
|
||||
phy->one_by_indq = b16divb16(b16ONE, ind_q);
|
||||
}
|
||||
352
libs/libdsp/lib_pid_b16.c
Normal file
352
libs/libdsp/lib_pid_b16.c
Normal file
@ -0,0 +1,352 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_pid_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_controller_init_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize PID controller. This function does not initialize saturation
|
||||
* limits.
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (out) pointer to the PID controller data
|
||||
* KP - (in) proportional gain
|
||||
* KI - (in) integral gain
|
||||
* KD - (in) derivative gain
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void pid_controller_init_b16(FAR pid_controller_b16_t *pid, b16_t KP,
|
||||
b16_t KI, b16_t KD)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
|
||||
/* Reset controller data */
|
||||
|
||||
memset(pid, 0, sizeof(pid_controller_b16_t));
|
||||
|
||||
/* Copy controller parameters */
|
||||
|
||||
pid->KP = KP;
|
||||
pid->KI = KI;
|
||||
pid->KD = KD;
|
||||
pid->KC = 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pi_controller_init_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize PI controller. This function does not initialize saturation
|
||||
* limits.
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (out) pointer to the PID controller data
|
||||
* KP - (in) proportional gain
|
||||
* KI - (in) integral gain
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void pi_controller_init_b16(FAR pid_controller_b16_t *pid, b16_t KP,
|
||||
b16_t KI)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
|
||||
/* Reset controller data */
|
||||
|
||||
memset(pid, 0, sizeof(pid_controller_b16_t));
|
||||
|
||||
/* Copy controller parameters */
|
||||
|
||||
pid->KP = KP;
|
||||
pid->KI = KI;
|
||||
pid->KD = 0;
|
||||
pid->KC = 0;
|
||||
|
||||
/* No windup-protection at default */
|
||||
|
||||
pid->aw_en = false;
|
||||
pid->ireset_en = false;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_saturation_set_b16
|
||||
*
|
||||
* Description:
|
||||
* Set controller saturation limits. Sometimes we need change saturation
|
||||
* configuration in the run-time, so this function is separate from
|
||||
* pid_controller_init().
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (out) pointer to the PID controller data
|
||||
* min - (in) lower limit
|
||||
* max - (in) upper limit
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void pid_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min,
|
||||
b16_t max)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
LIBDSP_DEBUGASSERT(min < max);
|
||||
|
||||
pid->sat.max = max;
|
||||
pid->sat.min = min;
|
||||
|
||||
/* Enable saturation in PID controller */
|
||||
|
||||
pid->pidsat_en = true;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pi_saturation_set_b16
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (out) pointer to the PID controller data
|
||||
* min - (in) lower limit
|
||||
* max - (in) upper limit
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void pi_saturation_set_b16(FAR pid_controller_b16_t *pid, b16_t min,
|
||||
b16_t max)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
LIBDSP_DEBUGASSERT(min < max);
|
||||
|
||||
pid->sat.max = max;
|
||||
pid->sat.min = min;
|
||||
|
||||
/* Enable saturation in PI controller */
|
||||
|
||||
pid->pisat_en = true;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_antiwindup_enable_b16
|
||||
****************************************************************************/
|
||||
|
||||
void pi_antiwindup_enable_b16(FAR pid_controller_b16_t *pid, b16_t KC,
|
||||
bool enable)
|
||||
{
|
||||
pid->aw_en = enable;
|
||||
pid->KC = KC;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_ireset_enable_b16
|
||||
****************************************************************************/
|
||||
|
||||
void pi_ireset_enable_b16(FAR pid_controller_b16_t *pid, bool enable)
|
||||
{
|
||||
pid->ireset_en = enable;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_integral_reset_b16
|
||||
****************************************************************************/
|
||||
|
||||
void pid_integral_reset_b16(FAR pid_controller_b16_t *pid)
|
||||
{
|
||||
pid->part[1] = 0;
|
||||
pid->aw = 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pi_integral_reset_b16
|
||||
****************************************************************************/
|
||||
|
||||
void pi_integral_reset_b16(FAR pid_controller_b16_t *pid)
|
||||
{
|
||||
pid_integral_reset_b16(pid);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pi_controller_b16
|
||||
*
|
||||
* Description:
|
||||
* PI controller with output saturation and windup protection
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (in/out) pointer to the PI controller data
|
||||
* err - (in) current controller error
|
||||
*
|
||||
* Returned Value:
|
||||
* Return controller output.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t pi_controller_b16(FAR pid_controller_b16_t *pid, b16_t err)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
|
||||
b16_t tmp = 0;
|
||||
|
||||
/* Store error in controller structure */
|
||||
|
||||
pid->err = err;
|
||||
|
||||
/* Get proportional part */
|
||||
|
||||
pid->part[0] = b16mulb16(pid->KP, err);
|
||||
|
||||
/* Get intergral part */
|
||||
|
||||
pid->part[1] += b16mulb16(pid->KI, (err - pid->aw));
|
||||
|
||||
/* Add proportional, integral */
|
||||
|
||||
pid->out = pid->part[0] + pid->part[1];
|
||||
|
||||
/* Store not saturated output */
|
||||
|
||||
tmp = pid->out;
|
||||
|
||||
/* Saturate output if enabled */
|
||||
|
||||
if (pid->pisat_en == true)
|
||||
{
|
||||
if (pid->out > pid->sat.max)
|
||||
{
|
||||
if (pid->ireset_en == true)
|
||||
{
|
||||
/* Reset I part */
|
||||
|
||||
if (err > 0)
|
||||
{
|
||||
pi_integral_reset_b16(pid);
|
||||
}
|
||||
}
|
||||
|
||||
/* Limit output to the upper limit */
|
||||
|
||||
pid->out = pid->sat.max;
|
||||
}
|
||||
else if (pid->out < pid->sat.min)
|
||||
{
|
||||
if (pid->ireset_en == true)
|
||||
{
|
||||
/* Reset I part */
|
||||
|
||||
if (err < 0)
|
||||
{
|
||||
pi_integral_reset_b16(pid);
|
||||
}
|
||||
}
|
||||
|
||||
/* Limit output to the lower limit */
|
||||
|
||||
pid->out = pid->sat.min;
|
||||
}
|
||||
}
|
||||
|
||||
/* Anti-windup I-part decay if enabled */
|
||||
|
||||
if (pid->aw_en == true)
|
||||
{
|
||||
pid->aw = b16mulb16(pid->KC, (tmp - pid->out));
|
||||
}
|
||||
|
||||
/* Return regulator output */
|
||||
|
||||
return pid->out;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pid_controller_b16
|
||||
*
|
||||
* Description:
|
||||
* PID controller with output saturation and windup protection
|
||||
*
|
||||
* Input Parameters:
|
||||
* pid - (in/out) pointer to the PID controller data
|
||||
* err - (in) current controller error
|
||||
*
|
||||
* Returned Value:
|
||||
* Return controller output.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
b16_t pid_controller_b16(FAR pid_controller_b16_t *pid, b16_t err)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(pid != NULL);
|
||||
|
||||
/* Get PI output */
|
||||
|
||||
pi_controller_b16(pid, err);
|
||||
|
||||
/* Get derivative part */
|
||||
|
||||
pid->part[2] = b16mulb16(pid->KD, (err - pid->err_prev));
|
||||
|
||||
/* Add derivative part to the PI part */
|
||||
|
||||
pid->out += pid->part[2];
|
||||
|
||||
/* Store current error */
|
||||
|
||||
pid->err_prev = err;
|
||||
|
||||
/* Saturate output if enabled */
|
||||
|
||||
if (pid->pidsat_en == true)
|
||||
{
|
||||
if (pid->out > pid->sat.max)
|
||||
{
|
||||
/* Limit output to the upper limit */
|
||||
|
||||
pid->out = pid->sat.max;
|
||||
}
|
||||
else if (pid->out < pid->sat.min)
|
||||
{
|
||||
/* Limit output to the lower limit */
|
||||
|
||||
pid->out = pid->sat.min;
|
||||
}
|
||||
}
|
||||
|
||||
/* Return regulator output */
|
||||
|
||||
return pid->out;
|
||||
}
|
||||
197
libs/libdsp/lib_pmsm_model_b16.c
Normal file
197
libs/libdsp/lib_pmsm_model_b16.c
Normal file
@ -0,0 +1,197 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_pmsm_model_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pmsm_model_initialize_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialzie FOC model
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int pmsm_model_initialize_b16(FAR struct pmsm_model_b16_s *model,
|
||||
FAR struct pmsm_phy_params_b16_s *phy,
|
||||
b16_t per)
|
||||
{
|
||||
DEBUGASSERT(model);
|
||||
DEBUGASSERT(phy);
|
||||
DEBUGASSERT(per > 0);
|
||||
|
||||
/* Copy motor model parameters */
|
||||
|
||||
memcpy(&model->phy, phy, sizeof(struct pmsm_phy_params_b16_s));
|
||||
|
||||
/* Initialize controller period */
|
||||
|
||||
model->per = per;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pmsm_model_elec_b16
|
||||
*
|
||||
* Description:
|
||||
* Update motor model electrical state
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int pmsm_model_elec_b16(FAR struct pmsm_model_b16_s *model,
|
||||
FAR ab_frame_b16_t *vab)
|
||||
{
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
b16_t tmp3 = 0;
|
||||
b16_t tmp4 = 0;
|
||||
b16_t tmp5 = 0;
|
||||
b16_t tmp6 = 0;
|
||||
|
||||
DEBUGASSERT(model);
|
||||
DEBUGASSERT(vab);
|
||||
|
||||
/* Copy alpha-beta voltage */
|
||||
|
||||
model->state.v_ab.a = vab->a;
|
||||
model->state.v_ab.b = vab->b;
|
||||
|
||||
/* Inverse Clarke transform - get abc voltage */
|
||||
|
||||
inv_clarke_transform_b16(&model->state.v_ab,
|
||||
&model->state.v_abc);
|
||||
|
||||
/* Park transform - get DQ voltage */
|
||||
|
||||
park_transform_b16(&model->state.angle.angle_el,
|
||||
&model->state.v_ab,
|
||||
&model->state.v_dq);
|
||||
|
||||
/* q current */
|
||||
|
||||
tmp1 = b16mulb16(model->phy.motor.res, model->state.i_dq.q);
|
||||
tmp2 = b16mulb16(model->phy.ind_d, model->state.i_dq.d);
|
||||
tmp3 = tmp2 + model->phy.flux_link;
|
||||
tmp4 = b16mulb16(model->state.omega_e, tmp3);
|
||||
tmp5 = model->state.v_dq.q - tmp1 - tmp4;
|
||||
tmp6 = b16mulb16(model->per, tmp5);
|
||||
|
||||
model->iq_int += b16mulb16(tmp6, model->phy.one_by_indq);
|
||||
|
||||
/* d current */
|
||||
|
||||
tmp1 = b16mulb16(model->phy.motor.res, model->state.i_dq.d);
|
||||
tmp2 = b16mulb16(model->phy.ind_q, model->state.i_dq.q);
|
||||
tmp3 = b16mulb16(tmp2, model->state.omega_e);
|
||||
tmp4 = model->state.v_dq.d - tmp1 + tmp3;
|
||||
tmp5 = b16mulb16(model->per, tmp4);
|
||||
|
||||
model->id_int += b16mulb16(tmp5, model->phy.one_by_indd);
|
||||
|
||||
/* Store currents */
|
||||
|
||||
model->state.i_dq.q = model->iq_int;
|
||||
model->state.i_dq.d = model->id_int;
|
||||
|
||||
/* Inverse Park transform - get alpha-beta current */
|
||||
|
||||
inv_park_transform_b16(&model->state.angle.angle_el,
|
||||
&model->state.i_dq,
|
||||
&model->state.i_ab);
|
||||
|
||||
/* Inverse Clarke transform - get abc current */
|
||||
|
||||
inv_clarke_transform_b16(&model->state.i_ab,
|
||||
&model->state.i_abc);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: pmsm_model_mech_b16
|
||||
*
|
||||
* Description:
|
||||
* Update motor model mechanical state
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
int pmsm_model_mech_b16(FAR struct pmsm_model_b16_s *model, b16_t load)
|
||||
{
|
||||
b16_t angle = 0;
|
||||
b16_t dir = 0;
|
||||
b16_t te = 0;
|
||||
b16_t tmp1 = 0;
|
||||
b16_t tmp2 = 0;
|
||||
b16_t tmp3 = 0;
|
||||
b16_t tmp4 = 0;
|
||||
b16_t tmp5 = 0;
|
||||
|
||||
DEBUGASSERT(model);
|
||||
|
||||
/* Get electrical torque developed by the motor */
|
||||
|
||||
tmp1 = model->phy.ind_d - model->phy.ind_q;
|
||||
tmp2 = b16mulb16(tmp1, model->state.i_dq.d);
|
||||
tmp3 = model->phy.flux_link - tmp2;
|
||||
tmp4 = b16mulb16((b16ONE + b16HALF), itob16(model->phy.motor.p));
|
||||
tmp5 = b16mulb16(tmp4, model->state.i_dq.q);
|
||||
|
||||
te = b16mulb16(tmp5, tmp3);
|
||||
|
||||
/* Get new mechanical velocity */
|
||||
|
||||
tmp1 = te - load;
|
||||
tmp2 = b16mulb16(model->per, tmp1);
|
||||
tmp3 = b16mulb16(tmp2, model->phy.one_by_iner);
|
||||
|
||||
model->state.omega_m = model->state.omega_m + tmp3;
|
||||
|
||||
/* Get new electrical velocity */
|
||||
|
||||
model->state.omega_e = b16mulb16(model->state.omega_m,
|
||||
itob16(model->phy.motor.p));
|
||||
|
||||
/* Get rotation direction */
|
||||
|
||||
dir = (model->state.omega_e > 0 ? DIR_CW_B16 : DIR_CCW_B16);
|
||||
|
||||
/* Update electrical angle */
|
||||
|
||||
tmp1 = b16mulb16(model->state.omega_e, model->per);
|
||||
|
||||
angle = model->state.angle.angle_el.angle + tmp1;
|
||||
|
||||
/* Update with mechanical angel */
|
||||
|
||||
motor_angle_e_update_b16(&model->state.angle, angle, dir);
|
||||
|
||||
return OK;
|
||||
}
|
||||
437
libs/libdsp/lib_svm_b16.c
Normal file
437
libs/libdsp/lib_svm_b16.c
Normal file
@ -0,0 +1,437 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_svm_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: svm3_sector_get_b16
|
||||
*
|
||||
* Description:
|
||||
* Get current sector for space vector modulation.
|
||||
*
|
||||
* Input Parameters:
|
||||
* ijk - (in) pointer to the auxiliary ABC frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static uint8_t svm3_sector_get_b16(FAR abc_frame_b16_t *ijk)
|
||||
{
|
||||
uint8_t sector = 0;
|
||||
b16_t i = ijk->a;
|
||||
b16_t j = ijk->b;
|
||||
b16_t k = ijk->c;
|
||||
|
||||
/* Identify the correct sector based on i,j,k frame:
|
||||
* 1. sector 1:
|
||||
* i > 0.0
|
||||
* j > 0.0
|
||||
* k <= 0.0
|
||||
* 2. sector 2:
|
||||
* i <= 0.0
|
||||
* j > 0.0
|
||||
* k <= 0.0
|
||||
* 3. sector 3:
|
||||
* i <= 0.0
|
||||
* j > 0.0
|
||||
* k > 0.0
|
||||
* 4. sector 4:
|
||||
* i <= 0.0
|
||||
* j <= 0.0
|
||||
* k > 0.0
|
||||
* 5. sector 5:
|
||||
* i > 0.0
|
||||
* j <= 0.0
|
||||
* k > 0.0
|
||||
* 6. sector 6:
|
||||
* i > 0.0
|
||||
* j <= 0.0
|
||||
* k <= 0.0
|
||||
*/
|
||||
|
||||
if (k <= 0)
|
||||
{
|
||||
if (i <= 0)
|
||||
{
|
||||
sector = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (j <= 0)
|
||||
{
|
||||
sector = 6;
|
||||
}
|
||||
else
|
||||
{
|
||||
sector = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (i <= 0)
|
||||
{
|
||||
if (j <= 0)
|
||||
{
|
||||
sector = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
sector = 3;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
sector = 5;
|
||||
}
|
||||
}
|
||||
|
||||
/* Return SVM sector */
|
||||
|
||||
return sector;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: svm3_duty_calc_b16
|
||||
*
|
||||
* Description:
|
||||
* Calculate duty cycles 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
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
static void svm3_duty_calc_b16(FAR struct svm3_state_b16_s *s,
|
||||
FAR abc_frame_b16_t *ijk)
|
||||
{
|
||||
b16_t i = ijk->a;
|
||||
b16_t j = ijk->b;
|
||||
b16_t k = ijk->c;
|
||||
b16_t T0 = 0;
|
||||
b16_t T1 = 0;
|
||||
b16_t T2 = 0;
|
||||
|
||||
/* Determine T1, T2 and T0 based on the sector */
|
||||
|
||||
switch (s->sector)
|
||||
{
|
||||
case 1:
|
||||
{
|
||||
T1 = i;
|
||||
T2 = j;
|
||||
break;
|
||||
}
|
||||
|
||||
case 2:
|
||||
{
|
||||
T1 = -k;
|
||||
T2 = -i;
|
||||
break;
|
||||
}
|
||||
|
||||
case 3:
|
||||
{
|
||||
T1 = j;
|
||||
T2 = k;
|
||||
break;
|
||||
}
|
||||
|
||||
case 4:
|
||||
{
|
||||
T1 = -i;
|
||||
T2 = -j;
|
||||
break;
|
||||
}
|
||||
|
||||
case 5:
|
||||
{
|
||||
T1 = k;
|
||||
T2 = i;
|
||||
break;
|
||||
}
|
||||
|
||||
case 6:
|
||||
{
|
||||
T1 = -j;
|
||||
T2 = -k;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
/* We should not get here */
|
||||
|
||||
LIBDSP_DEBUGASSERT(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Get null vector time */
|
||||
|
||||
T0 = b16ONE - T1 - T2;
|
||||
|
||||
/* Calculate duty cycle for 3 phase */
|
||||
|
||||
switch (s->sector)
|
||||
{
|
||||
case 1:
|
||||
{
|
||||
s->d_u = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_v = T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_w = b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
case 2:
|
||||
{
|
||||
s->d_u = T1 + b16mulb16(T0, b16HALF);
|
||||
s->d_v = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_w = b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
case 3:
|
||||
{
|
||||
s->d_u = b16mulb16(T0, b16HALF);
|
||||
s->d_v = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_w = T2 + b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
case 4:
|
||||
{
|
||||
s->d_u = b16mulb16(T0, b16HALF);
|
||||
s->d_v = T1 + b16mulb16(T0, b16HALF);
|
||||
s->d_w = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
case 5:
|
||||
{
|
||||
s->d_u = T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_v = b16mulb16(T0, b16HALF);
|
||||
s->d_w = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
case 6:
|
||||
{
|
||||
s->d_u = T1 + T2 + b16mulb16(T0, b16HALF);
|
||||
s->d_v = b16mulb16(T0, b16HALF);
|
||||
s->d_w = T1 + b16mulb16(T0, b16HALF);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
/* We should not get here */
|
||||
|
||||
LIBDSP_DEBUGASSERT(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: svm3_b16
|
||||
*
|
||||
* 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:
|
||||
*
|
||||
* |---------|-----------|--------------------|-----------------|
|
||||
* | Voltage | swithcing | Line to neutral | Line to line |
|
||||
* | vector | vectors | voltage | voltage |
|
||||
* | |-----------|--------------------|-----------------|
|
||||
* | | a | b | c | Van | Vbn | Vcn | Vab | Vbe | Vca |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V1 | 1 | 0 | 0 | 2/3 | -1/3 | -1/3 | 1 | 0 | -1 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V2 | 1 | 1 | 0 | 1/3 | 1/3 | -2/3 | 0 | 1 | -1 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V3 | 0 | 1 | 0 | -1/3 | 2/3 | -1/3 | -1 | 1 | 0 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V4 | 0 | 1 | 1 | -2/3 | 1/3 | 1/3 | -1 | 0 | 1 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V5 | 0 | 0 | 1 | -1/3 | -1/3 | 2/3 | 0 | -1 | 1 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V6 | 1 | 0 | 1 | 1/3 | -2/3 | 1/3 | 1 | -1 | 0 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
* | V7 | 1 | 1 | 1 | 0 | 0 | 0 | 0 | 0 | 0 |
|
||||
* |---------|---|---|---|------|------|------|-----|-----|-----|
|
||||
*
|
||||
* Voltage values given in relation to the bus voltage (Vbus)/
|
||||
*
|
||||
* Input Parameters:
|
||||
* s - (out) pointer to the SVM data
|
||||
* v_ab - (in) pointer to the modulation voltage vector in alpha-beta
|
||||
* frame, normalized to magnitude (0.0 - 1.0)
|
||||
*
|
||||
* NOTE: v_ab vector magnitude must be in range <0.0, 1.0> to get correct
|
||||
* SVM3 results.
|
||||
*
|
||||
* REFERENCE:
|
||||
* https://e2e.ti.com/group/motor/m/pdf_presentations/665547/download
|
||||
* pages 32-34
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void svm3_b16(FAR struct svm3_state_b16_s *s, FAR ab_frame_b16_t *v_ab)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(s != NULL);
|
||||
LIBDSP_DEBUGASSERT(v_ab != NULL);
|
||||
|
||||
abc_frame_b16_t ijk;
|
||||
|
||||
/* Perform modified inverse Clarke-transformation (alpha,beta) -> (i,j,k)
|
||||
* to obtain auxiliary frame which will be used in further calculations.
|
||||
*/
|
||||
|
||||
ijk.a = b16mulb16(-b16HALF, v_ab->b) + b16mulb16(SQRT3_BY_TWO_B16,
|
||||
v_ab->a);
|
||||
ijk.b = v_ab->b;
|
||||
ijk.c = -ijk.b - ijk.a;
|
||||
|
||||
/* Get vector sector */
|
||||
|
||||
s->sector = svm3_sector_get_b16(&ijk);
|
||||
|
||||
/* Get duty cycle */
|
||||
|
||||
svm3_duty_calc_b16(s, &ijk);
|
||||
|
||||
/* NOTE: we return not-saturated output. Duty-cycle saturation is
|
||||
* board-specific characteristic and we have not access to this
|
||||
* information here.
|
||||
*/
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: svm3_current_correct_b16
|
||||
*
|
||||
* Description:
|
||||
* Correct ADC samples (int32) according to SVM3 state.
|
||||
* NOTE: This works only with 3 shunt resistors configuration.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void svm3_current_correct_b16(FAR struct svm3_state_b16_s *s,
|
||||
b16_t *c0, b16_t *c1, b16_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_b16
|
||||
*
|
||||
* Description:
|
||||
* Initialize 3-phase SVM data.
|
||||
*
|
||||
* Input Parameters:
|
||||
* s - (in/out) pointer to the SVM state data
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void svm3_init_b16(FAR struct svm3_state_b16_s *s)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(s != NULL);
|
||||
|
||||
memset(s, 0, sizeof(struct svm3_state_b16_s));
|
||||
}
|
||||
151
libs/libdsp/lib_transform_b16.c
Normal file
151
libs/libdsp/lib_transform_b16.c
Normal file
@ -0,0 +1,151 @@
|
||||
/****************************************************************************
|
||||
* libs/libdsp/lib_transform_b16.c
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: clarke_transform_b16
|
||||
*
|
||||
* Description:
|
||||
* Clarke transform (abc frame -> ab frame).
|
||||
* Transform the abc frame to the alpha-beta frame.
|
||||
*
|
||||
* i_alpha = k*(i_a - 0.5*i_b - 0.5*i_c)
|
||||
* i_beta = k*sqrt(3)*0.5*(i_b - i_c)
|
||||
*
|
||||
* We assume that:
|
||||
* 1) k = 2/3 for the non-power-invariant transformation
|
||||
* 2) balanced system: a + b + c = 0
|
||||
*
|
||||
* Input Parameters:
|
||||
* abc - (in) pointer to the abc frame
|
||||
* ab - (out) pointer to the alpha-beta frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void clarke_transform_b16(FAR abc_frame_b16_t *abc,
|
||||
FAR ab_frame_b16_t *ab)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(abc != NULL);
|
||||
LIBDSP_DEBUGASSERT(ab != NULL);
|
||||
|
||||
ab->a = abc->a;
|
||||
ab->b = (b16mulb16(ONE_BY_SQRT3_B16, abc->a) +
|
||||
b16mulb16(TWO_BY_SQRT3_B16, abc->b));
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: inv_clarke_transform_b16
|
||||
*
|
||||
* Description:
|
||||
* Inverse Clarke transform (ab frame -> abc frame).
|
||||
* Transform the alpha-beta frame to the abc frame.
|
||||
*
|
||||
* Input Parameters:
|
||||
* ab - (in) pointer to the alpha-beta frame
|
||||
* abc - (out) pointer to the abc frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void inv_clarke_transform_b16(FAR ab_frame_b16_t *ab,
|
||||
FAR abc_frame_b16_t *abc)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(ab != NULL);
|
||||
LIBDSP_DEBUGASSERT(abc != NULL);
|
||||
|
||||
/* Assume non-power-invariant transform and balanced system */
|
||||
|
||||
abc->a = ab->a;
|
||||
abc->b = (b16mulb16(-b16HALF, ab->a) +
|
||||
b16mulb16(SQRT3_BY_TWO_B16, ab->b));
|
||||
abc->c = (-abc->a - abc->b);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: park_transform_b16
|
||||
*
|
||||
* Description:
|
||||
* Park transform (ab frame -> dq frame).
|
||||
* Transform the alpha-beta frame to the direct-quadrature frame.
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in) pointer to the phase angle data
|
||||
* ab - (in) pointer to the alpha-beta frame
|
||||
* dq - (out) pointer to the direct-quadrature frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void park_transform_b16(FAR phase_angle_b16_t *angle,
|
||||
FAR ab_frame_b16_t *ab,
|
||||
FAR dq_frame_b16_t *dq)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
LIBDSP_DEBUGASSERT(ab != NULL);
|
||||
LIBDSP_DEBUGASSERT(dq != NULL);
|
||||
|
||||
dq->d = b16mulb16(angle->cos, ab->a) + b16mulb16(angle->sin, ab->b);
|
||||
dq->q = b16mulb16(angle->cos, ab->b) - b16mulb16(angle->sin, ab->a);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: inv_park_transform_b16
|
||||
*
|
||||
* Description:
|
||||
* Inverse Park transform (dq frame -> ab frame).
|
||||
* Transform direct-quadrature frame to alpha-beta frame.
|
||||
*
|
||||
* Input Parameters:
|
||||
* angle - (in) pointer to the phase angle data
|
||||
* dq - (in) pointer to the direct-quadrature frame
|
||||
* ab - (out) pointer to the alpha-beta frame
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void inv_park_transform_b16(FAR phase_angle_b16_t *angle,
|
||||
FAR dq_frame_b16_t *dq,
|
||||
FAR ab_frame_b16_t *ab)
|
||||
{
|
||||
LIBDSP_DEBUGASSERT(angle != NULL);
|
||||
LIBDSP_DEBUGASSERT(dq != NULL);
|
||||
LIBDSP_DEBUGASSERT(ab != NULL);
|
||||
|
||||
ab->a = b16mulb16(angle->cos, dq->d) - b16mulb16(angle->sin, dq->q);
|
||||
ab->b = b16mulb16(angle->cos, dq->q) + b16mulb16(angle->sin, dq->d);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user