/*
* Algorithm.cla
*
* Created on: 2021年6月30日
* Author: xiecm
*/
/****************************************include*************************************/
#ifdef MATLAB_SIM
#include "adc.h"
//#include <../Application/global.h>
#include "Algorithm.h"
#else
#include <Sources/Application/global.h>
#endif
#include "CLAmath.h"
/****************************************define*************************************/
//SPLL参数
#define CLA_PI 3.1415926f
#define DELTA_TS (1.0f/ISR_FREQ) //1/60k
#define LPF_WC (1000) //1KHZ
#define GIRD_FREQ 50 //HZ
#define SPLL_Wf 30 //rad
#define SPLL_WfTs (SPLL_Wf * DELTA_TS)
#define SPLL_DDSRF_K1 (SPLL_WfTs/(2+SPLL_WfTs))
#define SPLL_DDSRF_K2 (2*SPLL_DDSRF_K1 - 1)
//#define CLA_TIMER_1MS (40)
//#define CLA_TIMER_5MS (200)
#define CLA_CMP_MIN 5
#define CLA_TIMER_8MS (480)
#define CLA_TIMER_10MS (600)
#define CLA_TIMER_20MS (1200)
#define CLA_TIMER_100MS (6000)
#define CLA_TIMER_500MS (30000)
#define _90DEG (1.5707963f)
#define Cla_Sqrt3 (1.732051f)
/****************************************Structure*************************************/
#ifdef MATLAB_SIM
CALIB_VALUE_CLA ClaCalibValue;//20250809_Check
ADCINSTANT_CLA ClaAdcInst;//20250809_Check
ADCRAW_CLA ClaAdcRaw;//20250809_Check
PH3DATA_CLA Cla3phData;//20250809_Check
PH3LOOP_CLA Cla3phLoopOut;//20250809_Check
ABC_DQ0_POS_CLA ClaInvUoutPos;//20250809_Check
ABC_DQ0_NEG_CLA ClaInvUoutNeg;//20250809_Check
ABC_DQ0_POS_CLA ClaIacDQ;//20250809_Check
DQ0_ABC_CLA ClaSpwm3ph;//20250809_Check
SPLL_3ph_DDSRF_CLA ClaGridUSpll;//20250809_Check
#else
CALIB_VALUE_CLA ClaCalibValue;//20250809_Check
ADCINSTANT_CLA ClaAdcInst;//20250809_Check
ADCRAW_CLA ClaAdcRaw;//20250809_Check
PH3DATA_CLA Cla3phData;//20250809_Check
PH3LOOP_CLA Cla3phLoopOut;//20250809_Check
ABC_DQ0_POS_CLA ClaInvUoutPos;//20250809_Check
ABC_DQ0_NEG_CLA ClaInvUoutNeg;//20250809_Check
ABC_DQ0_POS_CLA ClaIacDQ;//20250809_Check
DQ0_ABC_CLA ClaSpwm3ph;//20250809_Check
SPLL_3ph_DDSRF_CLA ClaGridUSpll;//20250809_Check
#endif
uint16_t PWMEnableFlagCLA ;//20250809_Check
uint16_t ClaIabcAverEnable;//20250809_Check
/*******************************************************************
* 输入：
* 输出：
* 作用：初始化CLA相关数据
*******************************************************************/
#pragma CODE_SECTION(InitClaTask7, "Cla1Prog");
#ifdef MATLAB_SIM
void InitClaTask7(void)
#else
interrupt void InitClaTask7(void)
#endif
{
ClaCalibValue.Ia2.f32Gain = K_GAIN_CALIBRATE;
ClaCalibValue.Ia2.f32Offset = 0.0;
ClaCalibValue.Ib2.f32Gain = K_GAIN_CALIBRATE;
ClaCalibValue.Ib2.f32Offset = 0.0;
ClaCalibValue.Ic2.f32Gain = K_GAIN_CALIBRATE;
ClaCalibValue.Ic2.f32Offset = 0.0;
ClaAdcInst.f32Ia2 = 0;
ClaAdcInst.f32Ia2Offset = 0;
ClaAdcInst.f32Ia2Offset1 = 0;
ClaAdcInst.f32Ib2 = 0;
ClaAdcInst.f32Ib2Offset = 0;
ClaAdcInst.f32Ib2Offset1 = 0;
ClaAdcInst.f32Ic2 = 0;
ClaAdcInst.f32Ic2Offset = 0;
ClaAdcInst.f32Ic2Offset1 = 0;
ClaAdcRaw.f32Ia2 = 0;
ClaAdcRaw.f32Ib2 = 0;
ClaAdcRaw.f32Ic2 = 0;
Cla3phData.clacnt = 0; //must be float type can run OK
Cla3phData.f32UaOutScale = 0; //电压采样>>11后的归一化值
Cla3phData.f32UbOutScale = 0;
Cla3phData.f32UcOutScale = 0;
Cla3phData.f32UabOutRms = 0; //市电线电压ab有效值
Cla3phData.f32UbcOutRms = 0; //市电线电压bc有效值
Cla3phData.f32UacOutRms = 0; //市电线电压ac有效值
Cla3phData.f32UabInvRms = 0; //逆变线电压ab有效值
Cla3phData.f32UbcInvRms = 0; //逆变线电压bc有效值
Cla3phData.f32UcaInvRms = 0; //逆变线电压ca有效值
Cla3phData.f32Ia = 0;
Cla3phData.f32Ib = 0;
Cla3phData.f32Ic = 0;
Cla3phData.i16SeqDir = 0; //相序顺序:ABC/ACB
Cla3phData.i16SeqDoneFlag = 0; //相序检测完成标识
Cla3phData.u16VabcLostCheck = 0; //市电瞬时采样检测失电
Cla3phData.i16Pfc3phStartState = 0; ////Cla 3phStartState
Cla3phData.u16MainsVLostFlag = 1; //市电失电标志位
Cla3phData.u16MainsVCheckRunOneFlag = 0;
Cla3phData.u16INVToGridLockDelayCnt = 0;
Cla3phData.FastMainLostCnt = 0;
Cla3phData.FastMainRecoverCnt = 0;
Cla3phData.f32Theta = 0; //市电相位
Cla3phData.f32GridTheta = 0; ///市电相位
Cla3phData.f32Omega = 0;
Cla3phData.u16AdjCnt = 0;
Cla3phData.u16InvPhaseLockCnt = 0;
Cla3phData.u16PllOkcnt = 0;
Cla3phData.u16Pllfailcnt = 0;
Cla3phData.bPhaseLocked = false;
Cla3phData.bInvPhaseLocked = false;
Cla3phData.f32InvUdloopout = 0;
Cla3phData.f32InvUqloopout = 0;
Cla3phData.f32InvUd_Pos = 0;
Cla3phData.f32InvUq_Pos = 0;
Cla3phData.f32InvUd_Pos_Ref = 0;
Cla3phData.debug2 = 0;
Cla3phData.WorkMode = BypassMode;
Cla3phData.FlagADCDcCompFinsh = 0;
Cla3phData.PR_ParaVaild = false;
Cla3phData.PR_ParaIndex = PR_AllFreq;
Cla3phData.PR_ParaErrFlag = false ;
Cla3phData.VdLoopPR_4Freq_Ka = 0;
Cla3phData.VdLoopPR_4Freq_Kb = 0;
Cla3phData.VdLoopPR_4Freq_Kc = 0;
Cla3phData.VdLoopPR_4Freq_Ku1 = 0;
Cla3phData.VdLoopPR_4Freq_Ku2 = 0;
Cla3phData.VqLoopPR_4Freq_Ka = 0;
Cla3phData.VqLoopPR_4Freq_Kb = 0;
Cla3phData.VqLoopPR_4Freq_Kc = 0;
Cla3phData.VqLoopPR_4Freq_Ku1 = 0;
Cla3phData.VqLoopPR_4Freq_Ku2 = 0;
Cla3phData.VdLoopPR_6Freq_Ka = 0;
Cla3phData.VdLoopPR_6Freq_Kb = 0;
Cla3phData.VdLoopPR_6Freq_Kc = 0;
Cla3phData.VdLoopPR_6Freq_Ku1 = 0;
Cla3phData.VdLoopPR_6Freq_Ku2 = 0;
Cla3phData.VqLoopPR_6Freq_Ka = 0;
Cla3phData.VqLoopPR_6Freq_Kb = 0;
Cla3phData.VqLoopPR_6Freq_Kc = 0;
Cla3phData.VqLoopPR_6Freq_Ku1 = 0;
Cla3phData.VqLoopPR_6Freq_Ku2 = 0;
Cla3phData.f32InvUdPRloopout = 0;
Cla3phData.f32InvUqPRloopout = 0;
Cla3phLoopOut.f32a = 0;
Cla3phLoopOut.f32b = 0;
Cla3phLoopOut.f32c = 0;
Cla3phLoopOut.f32vloopout = 0;
Cla3phLoopOut.f32limitMax = 0.8; //环路限值0.99
Cla3phLoopOut.f32limitAMax = 0;
Cla3phLoopOut.f32limitAMin = 0;
Cla3phLoopOut.f32limitBMax = 0;
Cla3phLoopOut.f32limitBMin = 0;
Cla3phLoopOut.f32limitCMax = 0;
Cla3phLoopOut.f32limitCMin = 0;
//Id 电流环
ClaId.Vars.Ref = 0;
ClaId.Vars.Output = 0;
ClaId.Vars.last_Output = 0;
ClaId.Vars.last2_Output = 0;
ClaId.Vars.last3_Output = 0;
ClaId.Vars.IntegralOutput = 0;
ClaId.Vars.Ek = 0;
ClaId.Vars.Ek1 = 0;
ClaId.Vars.Ek2 = 0;
ClaId.PiParas.kp = 0.008;///0.013;//0.017;//0.0025;
ClaId.PiParas.ki = 0.0004;///0.0005;//0.0001;
ClaId.PiParas.kd = 0.0;
ClaId.EkThreshold.Lower = -50;//-20;
ClaId.EkThreshold.Uper = 50;//20;
ClaId.PiOutThreshold.Lower = -1.24;
ClaId.PiOutThreshold.Uper = 1.24;
ClaId.IntegralThreshold.Lower = -1.24;
ClaId.IntegralThreshold.Uper = 1.24;
//Iq 电流环
ClaIq.Vars.Ref = 0;
ClaIq.Vars.Output = 0;
ClaIq.Vars.last_Output = 0;
ClaIq.Vars.last2_Output = 0;
ClaIq.Vars.last3_Output = 0;
ClaIq.Vars.IntegralOutput = 0;
ClaIq.Vars.Ek = 0;
ClaIq.Vars.Ek1 = 0;
ClaIq.Vars.Ek2 = 0;
ClaIq.PiParas.kp = 0.008;///0.013;//0.017;//0.01;
ClaIq.PiParas.ki = 0.0004;///0.0005;//0;//0.00005;
ClaIq.PiParas.kd = 0.0;
ClaIq.EkThreshold.Lower = -20;
ClaIq.EkThreshold.Uper = 20;
ClaIq.PiOutThreshold.Lower = -1;//-0.5;
ClaIq.PiOutThreshold.Uper = 1;//0.5;
ClaIq.IntegralThreshold.Lower = -1;//-0.5;
ClaIq.IntegralThreshold.Uper = 1;//0.5;
ClaSpwm3ph.a = 0;
ClaSpwm3ph.b = 0;
ClaSpwm3ph.c = 0;
ClaSpwm3ph.alpha = 0;
ClaSpwm3ph.beta = 0;
ClaSpwm3ph.sin_out = 0;
ClaSpwm3ph.cos_out = 0;
ClaSpwm3ph.d = 0;
ClaSpwm3ph.q = 0;
ClaSpwm3ph.z =0;
// ClaGridUSpll.U32PllRstCn = 0;
// ClaGridUSpll.cos90deg = 0;
// ClaGridUSpll.cos_2theta = 0;
// ClaGridUSpll.cos_theta = 1.0f;//TODO
// ClaGridUSpll.d_n = 0;
// ClaGridUSpll.d_n_decoupl = 0;
// ClaGridUSpll.d_p = 0;
// ClaGridUSpll.d_p_decoupl = 0;
// ClaGridUSpll.d_p_decoupl_lpf = 0;
// ClaGridUSpll.delta_T = 0;
// ClaGridUSpll.fn = 50;
// ClaGridUSpll.fo = 0;
// ClaGridUSpll.k1 = 0;
// ClaGridUSpll.k2 = 0;
// ClaGridUSpll.lpf_coeff.A1_lf = 0;
// ClaIacDQ.a = 0;
// ClaIacDQ.b = 0;
// ClaIacDQ.c = 0;
// ClaIacDQ.alpha = 0;
// ClaIacDQ.beta = 0;
// ClaIacDQ.sinout = 0;
// ClaIacDQ.cosout = 0;
// ClaIacDQ.d = 0;
// ClaIacDQ.q = 0;
// ClaIacDQ.z = 0;
ABC_DQ0_POS_CLA_init(ClaIacDQ);
//ABC-->DQP/N变换
ABC_DQ0_POS_CLA_init(ClaInvUoutPos);
// ABC_DQ0_NEG_CLA_init(ClaInvUoutNeg);
//三相pll算法初始化
SPLL_3ph_DDSRF_CLA_init(GIRD_FREQ, DELTA_TS, SPLL_DDSRF_K1, SPLL_DDSRF_K2, ClaGridUSpll);
//三相pll算法
SPLL_3ph_F_init(GIRD_FREQ, DELTA_TS, &Pll3ph);
SPLL_3ph_DSOGI_F_coeff_update(DELTA_TS, GIRD_FREQ, &DSogiGridAlpha);
SPLL_3ph_DSOGI_F_coeff_update(DELTA_TS, GIRD_FREQ, &DSogiGridBeta);
ClaToCpu.f32GridFreq = 0;
ClaToCpu.bPhaseLocked = 0;
ClaToCpu.bInvPhaseLocked = 0;
ClaToCpu.f32CosTheta90Deg = 0; //锁相与电网相差90度，滞后
ClaToCpu.f32SinTheta90Deg = 0;
ClaToCpu.f32Theta = 0;
ClaToCpu.f32CosTheta = 1; //
ClaToCpu.f32SinTheta = 0;
ClaToCpu.f32LoopCoeff = 0; //前馈加入环路中系数
ClaToCpu.Grid_PosPower2_Ud = 0;///电网正序Ud
ClaToCpu.Grid_PosPower2_Uq = 0;///电网正序Uq
ClaToCpu.Grid_PosPower2_LP100HzUd = 0;
ClaToCpu.Grid_PosPower2_LP100HzUq = 0;
ClaToCpu.f32Ia2 = 0;
ClaToCpu.f32Ib2 = 0;
ClaToCpu.f32Ic2 = 0;
ClaToCpu.f32I2AbsMax = 0;
ClaToCpu.f32Ia2Offset = 0;
ClaToCpu.f32Ib2Offset = 0;
ClaToCpu.f32Ic2Offset = 0;
ClaToCpu.f32Pos_Id = 0;
ClaToCpu.f32Pos_Iq = 0;
ClaToCpu.f32Pos_LP100HzId = 0;
ClaToCpu.f32Pos_LP100HzIq = 0;
ClaToCpu.u16MainsVLostFlag = 0;
ClaToCpu.u16InvVLostFlag = 0;
ClaToCpu.f32InvUdPRloopout = 0;
ClaToCpu.f32InvUqPRloopout = 0;
//PR电流环1
// ClaPRLoopId1.Wc = 300;//500;
// ClaPRLoopId1.Kr = 0.03;//0.017;
// ClaPRLoopId1.Kp = 0;
// ClaPRId1.EkThreshold.Lower = -50;
// ClaPRId1.EkThreshold.Uper = 50;
// ClaPRId1.PiOutThreshold.Lower = -1;
// ClaPRId1.PiOutThreshold.Uper = 1;
//
// ClaPRLoopIq1.Wc = 200;
// ClaPRLoopIq1.Kr = 0.02;//0.002;
// ClaPRLoopIq1.Kp = 0;
// ClaPRIq1.EkThreshold.Lower = -20;
// ClaPRIq1.EkThreshold.Uper = 20;
// ClaPRIq1.PiOutThreshold.Lower = -0.5;
// ClaPRIq1.PiOutThreshold.Uper = 0.5;
//
// Cla_PR_Para_Cal(&ClaPRId1, 300, DELTA_TS, ClaPRLoopId1.Wc, ClaPRLoopId1.Kr, ClaPRLoopId1.Kp);
// Cla_PR_Para_Cal(&ClaPRIq1, 300, DELTA_TS, ClaPRLoopIq1.Wc, ClaPRLoopIq1.Kr, ClaPRLoopIq1.Kp);
ClaPRVdPara_4Freq.Wc = 200;
ClaPRVdPara_4Freq.Kr = 2.5;
ClaPRVdPara_4Freq.Kp = 0.00;
ClaPRVqPara_4Freq.Wc = 20;
ClaPRVqPara_4Freq.Kr = 30;
ClaPRVqPara_4Freq.Kp = 0.00;
ClaPRVdPara_6Freq.Wc = 300;
ClaPRVdPara_6Freq.Kr = 2.5;
ClaPRVdPara_6Freq.Kp = 0.00;
ClaPRVqPara_6Freq.Wc = 30;
ClaPRVqPara_6Freq.Kr = 30;
ClaPRVqPara_6Freq.Kp = 0.00;
Cla_PR_Para_Cal(&ClaVdLoopPR_4Freq,200, DELTA_TS, ClaPRVdPara_4Freq.Wc, ClaPRVdPara_4Freq.Kr, ClaPRVdPara_4Freq.Kp);
Cla_PR_Para_Cal(&ClaVqLoopPR_4Freq,200, DELTA_TS, ClaPRVqPara_4Freq.Wc, ClaPRVqPara_4Freq.Kr, ClaPRVqPara_4Freq.Kp);
Cla_PR_Para_Cal(&ClaVdLoopPR_6Freq,300, DELTA_TS, ClaPRVdPara_6Freq.Wc, ClaPRVdPara_6Freq.Kr, ClaPRVdPara_6Freq.Kp);
Cla_PR_Para_Cal(&ClaVqLoopPR_6Freq,300, DELTA_TS, ClaPRVqPara_6Freq.Wc, ClaPRVqPara_6Freq.Kr, ClaPRVqPara_6Freq.Kp);
ClaVdLoopPR_4Freq.Vars.Output = 0;
ClaVdLoopPR_4Freq.EkThreshold.Lower = PR_D_LIMIT_LOW;//TODO//InvUdLoop.EkThreshold.Lower;
ClaVdLoopPR_4Freq.EkThreshold.Uper = PR_D_LIMIT_UP;//TODO//InvUdLoop.EkThreshold.Uper;
ClaVdLoopPR_4Freq.PiOutThreshold.Lower = PR_D_LIMIT_LOW;
ClaVdLoopPR_4Freq.PiOutThreshold.Uper = PR_D_LIMIT_UP;
ClaVqLoopPR_4Freq.Vars.Output = 0;
ClaVqLoopPR_4Freq.EkThreshold.Lower = PR_Q_LIMIT_LOW;//TODO//InvUqLoop.EkThreshold.Lower;
ClaVqLoopPR_4Freq.EkThreshold.Uper = PR_Q_LIMIT_UP;//TODO//InvUqLoop.EkThreshold.Uper;
ClaVqLoopPR_4Freq.PiOutThreshold.Lower = PR_Q_LIMIT_LOW;
ClaVqLoopPR_4Freq.PiOutThreshold.Uper = PR_Q_LIMIT_UP;
ClaVdLoopPR_6Freq.Vars.Output = 0;
ClaVdLoopPR_6Freq.EkThreshold.Lower = PR_D_LIMIT_LOW;//TODO//InvUdLoop.EkThreshold.Lower;
ClaVdLoopPR_6Freq.EkThreshold.Uper = PR_D_LIMIT_UP;//TODO//InvUdLoop.EkThreshold.Uper;
ClaVdLoopPR_6Freq.PiOutThreshold.Lower = PR_D_LIMIT_LOW;
ClaVdLoopPR_6Freq.PiOutThreshold.Uper = PR_D_LIMIT_UP;
ClaVqLoopPR_6Freq.EkThreshold.Lower = PR_Q_LIMIT_LOW;//TODO//InvUqLoop.EkThreshold.Lower;
ClaVqLoopPR_6Freq.EkThreshold.Uper = PR_Q_LIMIT_UP;//TODO//InvUqLoop.EkThreshold.Uper;
ClaVqLoopPR_6Freq.PiOutThreshold.Lower = PR_Q_LIMIT_LOW;
ClaVqLoopPR_6Freq.PiOutThreshold.Uper = PR_Q_LIMIT_UP;
ClaVqLoopPR_6Freq.Vars.Output = 0;
PWMEnableFlagCLA = 0;
ClaIabcAverEnable = 0;
}/*InitClaTask7*/
/********************************************************************
* 输入:
* 输出:
* 作用：复位锁相环参数
* 说明：
* ******************************************************************/
void Spll_3ph_DDSRF_Reset(SPLL_3ph_DDSRF_CLA *spll_obj)
{
spll_obj->d_p = (float32)(0.0);
spll_obj->d_n = (float32)(0.0);
spll_obj->q_p = (float32)(0.0);
spll_obj->q_n = (float32)(0.0);
spll_obj->d_p_decoupl = (float32)(0.0);
spll_obj->d_n_decoupl = (float32)(0.0);
spll_obj->q_p_decoupl = (float32)(0.0);
spll_obj->q_n_decoupl = (float32)(0.0);
spll_obj->d_p_decoupl_lpf = (float32)(0.0);
spll_obj->d_n_decoupl_lpf = (float32)(0.0);
spll_obj->q_p_decoupl_lpf = (float32)(0.0);
spll_obj->q_n_decoupl_lpf = (float32)(0.0);
spll_obj->y[0] = (float32)(0.0);
spll_obj->y[1] = (float32)(0.0);
spll_obj->x[0] = (float32)(0.0);
spll_obj->x[1] = (float32)(0.0);
spll_obj->w[0] = (float32)(0.0);
spll_obj->w[1] = (float32)(0.0);
spll_obj->z[0] = (float32)(0.0);
spll_obj->z[1] = (float32)(0.0);
spll_obj->v_q[0] = (float32)(0.0);
spll_obj->v_q[1] = (float32)(0.0);
spll_obj->ylf[0] = (float32)(0.0);
spll_obj->ylf[1] = (float32)(0.0);
spll_obj->theta[0] = (float32)(0.0);
spll_obj->theta[1] = (float32)(0.0);
}/*Spll_3ph_DDSRF_Reset*/
/********************************************************************
* 输入:
* 输出:
* 作用：复位SOGI参数
* 说明：
* ******************************************************************/
void Spll_3ph_DSOGI_Reset(SPLL_3ph_DDSOGI_F *spll_obj)
{
spll_obj->u[0]=(float32)(0.0);
spll_obj->u[1]=(float32)(0.0);
spll_obj->u[2]=(float32)(0.0);
spll_obj->osg_u[0]=(float32)(0.0);
spll_obj->osg_u[1]=(float32)(0.0);
spll_obj->osg_u[2]=(float32)(0.0);
spll_obj->osg_qu[0]=(float32)(0.0);
spll_obj->osg_qu[1]=(float32)(0.0);
spll_obj->osg_qu[2]=(float32)(0.0);
spll_obj->u_Q[0]=(float32)(0.0);
spll_obj->u_Q[1]=(float32)(0.0);
spll_obj->u_D[0]=(float32)(0.0);
spll_obj->u_D[1]=(float32)(0.0);
}/*Spll_3ph_DDSRF_Reset*/
/********************************************************************
* 输入:
* 输出:
* 作用：复位锁相环参数
* 说明：
* ******************************************************************/
void SPLL_3ph_F_Reset(SPLL_3ph_VAR *spll_obj)
{
spll_obj->osg_u[0]=(float32)(0.0);
spll_obj->osg_u[1]=(float32)(0.0);
spll_obj->osg_u[2]=(float32)(0.0);
spll_obj->osg_qu[0]=(float32)(0.0);
spll_obj->osg_qu[1]=(float32)(0.0);
spll_obj->osg_qu[2]=(float32)(0.0);
spll_obj->u_Q[0]=(float32)(0.0);
spll_obj->u_Q[1]=(float32)(0.0);
spll_obj->u_D[0]=(float32)(0.0);
spll_obj->u_D[1]=(float32)(0.0);
spll_obj->ylf[0]=(float32)(0.0);
spll_obj->ylf[1]=(float32)(0.0);
spll_obj->fo=(float32)(0.0);
spll_obj->theta[0]=(float32)(0.0);
spll_obj->theta[1]=(float32)(0.0);
spll_obj->sin_out=(float32)(0.0);
spll_obj->cos_out=(float32)(0.0);
}/*SPLL_3ph_F_RESET*/
/*******************************************************************
* 输入：
* 输出：
* 作用：复位环路参数
* 执行时间:30k
*******************************************************************/
void ClaLoopReset(void)
{
//给定复位
ClaId.Vars.Ref = 0;
ClaIq.Vars.Ref = 0;
//Id电流环
ClaId.Vars.Ek = 0;
ClaId.Vars.IntegralOutput = 0;//-0.5;//-0.35;//-1.22;
ClaId.Vars.Output = 0;//-0.5;//-0.35;//-1.22; //mos最小占空比输出，防过冲？
//Iq电流环
ClaIq.Vars.Ek = 0;
ClaIq.Vars.IntegralOutput = 0;
ClaIq.Vars.Output = 0;
//环路限值
Cla3phLoopOut.f32limitMax = 0.8;
// //PR电流环1
// ClaPRId1.Vars.Ek = 0;
// ClaPRId1.Vars.Ek1 = 0;
// ClaPRId1.Vars.Ek2 = 0;
// ClaPRId1.Vars.Output = 0;
// ClaPRId1.Vars.Output1 = 0;
// ClaPRId1.Vars.Output2 = 0;
//
// ClaPRIq1.Vars.Ek = 0;
// ClaPRIq1.Vars.Ek1 = 0;
// ClaPRIq1.Vars.Ek2 = 0;
// ClaPRIq1.Vars.Output = 0;
// ClaPRIq1.Vars.Output1 = 0;
// ClaPRIq1.Vars.Output2 = 0;
ClaToCpu.f32LoopCoeff = 1.0f;
}/*ClaLoopReset*/
/*******************************************************************
* 输入：
* 输出：
* 作用：读取从cpu传递过来的数据
* 执行时间:30k
*******************************************************************/
void ClaReadCpuData(void)
{
Cla3phData.f32UaOutScale = CpuToCla.f32UaOutScale; //市电电压Ua
Cla3phData.f32UbOutScale = CpuToCla.f32UbOutScale; //市电电压Ub
Cla3phData.f32UcOutScale = CpuToCla.f32UcOutScale; //市电电压Uc
Cla3phData.f32UabOutRms = CpuToCla.f32UabOutRms; //市电线电压Uab有效值
Cla3phData.f32UbcOutRms = CpuToCla.f32UbcOutRms; //市电线电压Ubc有效值
Cla3phData.f32UacOutRms = CpuToCla.f32UcaOutRms; //市电线电压Uac有效值
Cla3phData.f32UabInvRms = CpuToCla.f32UabInvRms;
Cla3phData.f32UbcInvRms = CpuToCla.f32UbcInvRms;
Cla3phData.f32UcaInvRms = CpuToCla.f32UcaInvRms;
Cla3phData.i16SeqDir = CpuToCla.i16SeqDir; //相序顺序:正负序
Cla3phData.i16SeqDoneFlag = CpuToCla.i16SeqDoneFlag; //相序计算完成标识
Cla3phData.u16VabcLostCheck = CpuToCla.u16VabcLostCheck;
Cla3phData.f32InvUdloopout = CpuToCla.f32InvUdloopout;
Cla3phData.f32InvUqloopout = CpuToCla.f32InvUqloopout;
Cla3phData.i16Pfc3phStartState = CpuToCla.i16Pfc3phStartState;
ClaCalibValue.Ia2.f32Gain = CpuToCla.f32Ia2Gain;
ClaCalibValue.Ia2.f32Offset = CpuToCla.f32Ia2Offset;
ClaCalibValue.Ib2.f32Gain = CpuToCla.f32Ib2Gain;
ClaCalibValue.Ib2.f32Offset = CpuToCla.f32Ib2Offset;
ClaCalibValue.Ic2.f32Gain = CpuToCla.f32Ic2Gain;
ClaCalibValue.Ic2.f32Offset = CpuToCla.f32Ic2Offset;
//环路参数传递
ClaId.PiParas.kp = CpuToCla.f32DIKp;
ClaId.PiParas.ki = CpuToCla.f32DIKi;
// Cla3phData.bGridRealLost = CpuToCla.bGridRealLost;
ClaIq.PiParas.kp = CpuToCla.f32QIKp;
ClaIq.PiParas.ki = CpuToCla.f32QIKi;
// Cla3phData.WorkMode = CpuToCla.WorkMode;
// Cla3phData.CBC_State = CpuToCla.CBC_State;
Cla3phData.FlagADCDcCompFinsh = CpuToCla.FlagADCDcCompFinsh;
// ClaPRLoopId1.Wc = CpuToCla.f32PRIdWc1;
// ClaPRLoopId1.Kr = CpuToCla.f32PRIdKr1;
// ClaPRLoopIq1.Wc = CpuToCla.f32PRIqWc1;
// ClaPRLoopIq1.Kr = CpuToCla.f32PRIqKr1;
Cla3phData.f32InvUd_Pos = CpuToCla.f32InvUd_Pos;
Cla3phData.f32InvUq_Pos = CpuToCla.f32InvUq_Pos;
Cla3phData.f32InvUd_Pos_Ref = CpuToCla.f32InvUd_Pos_Ref;
Cla3phData.PR_ParaVaild = CpuToCla.PR_ParaVaild;
Cla3phData.PR_ParaIndex = CpuToCla.PR_ParaIndex;
Cla3phData.PR_ParaIndex = PR_AllFreq;
Cla3phData.PR_ParaErrFlag = CpuToCla.PR_ParaErrFlag;
// ClaVdLoopPR_4Freq.PiParas.ka = CpuToCla.VdLoopPR_4Freq_Ka;
// ClaVdLoopPR_4Freq.PiParas.kb = CpuToCla.VdLoopPR_4Freq_Kb;
// ClaVdLoopPR_4Freq.PiParas.kc = CpuToCla.VdLoopPR_4Freq_Kc;
// ClaVdLoopPR_4Freq.PiParas.ku1 = CpuToCla.VdLoopPR_4Freq_Ku1;
// ClaVdLoopPR_4Freq.PiParas.ku2 = CpuToCla.VdLoopPR_4Freq_Ku2;
//
// ClaVqLoopPR_4Freq.PiParas.ka = CpuToCla.VqLoopPR_4Freq_Ka;
// ClaVqLoopPR_4Freq.PiParas.kb = CpuToCla.VqLoopPR_4Freq_Kb;
// ClaVqLoopPR_4Freq.PiParas.kc = CpuToCla.VqLoopPR_4Freq_Kc;
// ClaVqLoopPR_4Freq.PiParas.ku1 = CpuToCla.VqLoopPR_4Freq_Ku1;
// ClaVqLoopPR_4Freq.PiParas.ku2 = CpuToCla.VqLoopPR_4Freq_Ku2;
//
// ClaVdLoopPR_6Freq.PiParas.ka = CpuToCla.VdLoopPR_6Freq_Ka;
// ClaVdLoopPR_6Freq.PiParas.kb = CpuToCla.VdLoopPR_6Freq_Kb;
// ClaVdLoopPR_6Freq.PiParas.kc = CpuToCla.VdLoopPR_6Freq_Kc;
// ClaVdLoopPR_6Freq.PiParas.ku1 = CpuToCla.VdLoopPR_6Freq_Ku1;
// ClaVdLoopPR_6Freq.PiParas.ku2 = CpuToCla.VdLoopPR_6Freq_Ku2;
//
// ClaVqLoopPR_6Freq.PiParas.ka = CpuToCla.VqLoopPR_6Freq_Ka;
// ClaVqLoopPR_6Freq.PiParas.kb = CpuToCla.VqLoopPR_6Freq_Kb;
// ClaVqLoopPR_6Freq.PiParas.kc = CpuToCla.VqLoopPR_6Freq_Kc;
// ClaVqLoopPR_6Freq.PiParas.ku1 = CpuToCla.VqLoopPR_6Freq_Ku1;
// ClaVqLoopPR_6Freq.PiParas.ku2 = CpuToCla.VqLoopPR_6Freq_Ku2;
}/*ClaReadCpuData()*/
/*******************************************************************
* 输入：
* 输出：
* 作用：cla传递数据至cpu
* 执行时间:60k
*******************************************************************/
void ClaSendCpuData(void)
{
ClaToCpu.bPhaseLocked = Cla3phData.bPhaseLocked; //锁相标识
ClaToCpu.bInvPhaseLocked = Cla3phData.bInvPhaseLocked;
ClaToCpu.f32GridFreq = Pll3ph.fo; //pll frequency
ClaToCpu.f32Theta = Cla3phData.f32Theta;
ClaToCpu.f32CosTheta = ClaGridUSpll.cos_theta;//ClaInvUoutPos.cosout; //cos(theta)
ClaToCpu.f32SinTheta = ClaGridUSpll.sin_theta;//ClaInvUoutPos.sinout; //sin(theta),与GridUa同频
ClaToCpu.f32Ia2 = ClaAdcRaw.f32Ia2;
ClaToCpu.f32Ib2 = ClaAdcRaw.f32Ib2;
ClaToCpu.f32Ic2 = ClaAdcRaw.f32Ic2;
ClaToCpu.f32Ia2Offset = ClaAdcInst.f32Ia2Offset;
ClaToCpu.f32Ib2Offset = ClaAdcInst.f32Ib2Offset;
ClaToCpu.f32Ic2Offset = ClaAdcInst.f32Ic2Offset;
ClaToCpu.u16MainsVLostFlag = Cla3phData.u16MainsVLostFlag;
ClaToCpu.f32InvUdPRloopout = Cla3phData.f32InvUdPRloopout;
ClaToCpu.f32InvUqPRloopout = Cla3phData.f32InvUqPRloopout;
// ClaToCpu.Grid_PosPower2_Ud = (Pll3ph.u_D[0]*4194304*Pll3ph.u_D[0]);///TODO//2048*2048 = 4194304
// ClaToCpu.Grid_PosPower2_Uq = (Pll3ph.u_Q[0]*4194304*Pll3ph.u_Q[0]);///TODO///V1105_10
// ClaToCpu.f32Pos_Id = ClaIacDQ.d;
// ClaToCpu.f32Pos_Iq = ClaIacDQ.q;
}/*ClaSendCpuData()*/
/*******************************************************************
* 输入：
* 输出：
* 作用：市电失电检测
* 执行时间:60k
*******************************************************************/
void ClaMainsVoltCheck(void)
{
#define DELAY_INVTOLOCKGRID 60000
#define MAINS_LOST_V 150 //市电失电值
#define MAX_THETA 0.209333f///0.104667f
#define ADJ_THETA 0.017444f
#define SPLL_DELTA_THETA 0.10471975f/// 20*DELTA_TS*2.0 * CLA_PI * 50
#define SPLL_ADJ_THETA 0.005235988f///2.0 * CLA_PI * 50*DELTA_TS
#define ADJ_CNT 40 ///1200*6*SPLL_ADJ_THETA/(40*16.7us) ≈ 0.05643 //一个周期最多修正的弧度值///ADJ_SPEED 为20时 最多可以修正0.1的弧度值，同样 将
#define INVLOCK_CNT 2
//根据市电线电压有效值判断是否失电
// if((Cla3phData.f32UabOutRms < MAINS_LOST_V) && (Cla3phData.f32UbcOutRms < MAINS_LOST_V) && (Cla3phData.f32UacOutRms < MAINS_LOST_V))
if(//(fabs(ClaInvUoutPos.d) < 0.08) ||
((Cla3phData.f32UabOutRms < MAINS_LOST_V) && (Cla3phData.f32UbcOutRms < MAINS_LOST_V) && (Cla3phData.f32UacOutRms < MAINS_LOST_V)))
{
Cla3phData.FastMainLostCnt ++;
Cla3phData.FastMainRecoverCnt = 0;
if(Cla3phData.FastMainLostCnt > 20000)
{
Cla3phData.FastMainLostCnt = 20000;//300ms左右的检测时间视为稳定掉电
Cla3phData.u16MainsVLostFlag = 1;
}
}
else
{
Cla3phData.FastMainRecoverCnt ++;
Cla3phData.FastMainLostCnt = 0;
if(Cla3phData.FastMainRecoverCnt > 60000)//900MS
{
Cla3phData.FastMainRecoverCnt = 60000;
Cla3phData.u16MainsVLostFlag = 0;
}
}
if(Cla3phData.u16MainsVLostFlag == 1)
{
Cla3phData.bPhaseLocked = false; //通过锁相环锁相，先置锁相失败
Cla3phData.bInvPhaseLocked = false; //通过锁相环锁相，先置锁相失败
//if(Cla3phData.u16MainsVCheckRunOneFlag == 0) //只赋值一次
//{
Cla3phData.u16MainsVCheckRunOneFlag = 1;
// Cla3phData.f32Theta = Pll3ph.theta[2];
//Cla3phData.bPhaseLocked = true; //自算相位，直接置锁相成功
//}
if(Cla3phData.i16Pfc3phStartState >= PfcChargeOn)
{
//相位自算
Cla3phData.f32Omega = 2.0 * CLA_PI * 50;
Cla3phData.f32Theta += Cla3phData.f32Omega * DELTA_TS;
if(Cla3phData.f32Theta >= 2 * CLA_PI)
{
Cla3phData.f32Theta -= 2 * CLA_PI;
}
ClaGridUSpll.sin_theta = CLAsin(Cla3phData.f32Theta);
ClaGridUSpll.cos_theta = CLAcos(Cla3phData.f32Theta);
}
else
{
Cla3phData.bInvPhaseLocked = false;
Cla3phData.f32Theta = 0;
}
}
else
{
if(CpuToCla.bBypassOverTrack)
{
Cla3phData.bInvPhaseLocked = false;
Cla3phData.u16AdjCnt = 0;
}
else
{
if(Cla3phData.u16AdjCnt < 1000)
Cla3phData.u16AdjCnt++;
}
//Cla3phData.u16MainsVLostFlag = 0;
if(false == Cla3phData.bInvPhaseLocked)
{
Cla3phData.f32GridTheta = Pll3ph.theta[2];
if(Cla3phData.i16Pfc3phStartState >= PfcChargeOn)
{
if(CpuToCla.bBypassOverTrack || Cla3phData.u16INVToGridLockDelayCnt < DELAY_INVTOLOCKGRID)
{ //相位自算
if(Cla3phData.u16INVToGridLockDelayCnt < DELAY_INVTOLOCKGRID)
Cla3phData.u16INVToGridLockDelayCnt ++;
Cla3phData.f32Omega = 2.0 * CLA_PI * 50;
Cla3phData.f32Theta += Cla3phData.f32Omega * DELTA_TS;
if(Cla3phData.f32Theta >= 2 * CLA_PI)
{
Cla3phData.f32Theta -= 2 * CLA_PI;
}
}
else
{
Cla3phData.f32Omega = 2.0 * CLA_PI * Pll3ph.fo;
Cla3phData.f32Theta += Cla3phData.f32Omega * DELTA_TS;
if(Cla3phData.f32Theta >= 2 * CLA_PI)
{
Cla3phData.f32Theta -= 2 * CLA_PI;
}
}
ClaGridUSpll.sin_theta = CLAsin(Cla3phData.f32Theta);
ClaGridUSpll.cos_theta = CLAcos(Cla3phData.f32Theta);
}
else
{
Cla3phData.u16INVToGridLockDelayCnt = 0;
Cla3phData.f32Theta = 0;
}
if(((( Cla3phData.f32Theta > 0.42) && ( Cla3phData.f32Theta < 0.62)) ||
(( Cla3phData.f32Theta > 1.47) && ( Cla3phData.f32Theta < 1.67)) ||
(( Cla3phData.f32Theta > 2.51) && ( Cla3phData.f32Theta < 2.71)) ||
(( Cla3phData.f32Theta > 3.56) && ( Cla3phData.f32Theta < 3.76)) ||
(( Cla3phData.f32Theta > 4.61) && ( Cla3phData.f32Theta < 4.81)) ||
(( Cla3phData.f32Theta > 5.66) && ( Cla3phData.f32Theta < 5.86))) && Cla3phData.u16AdjCnt > ADJ_CNT && 0 == CpuToCla.bBypassOverTrack && Cla3phData.u16INVToGridLockDelayCnt >= DELAY_INVTOLOCKGRID)
// Cla3phData.f32UabInvRms > MAINS_LOST_V && Cla3phData.f32UbcInvRms > MAINS_LOST_V && Cla3phData.f32UcaInvRms > MAINS_LOST_V && Cla3phData.u16INVToGridLockDelayCnt >= 60000)
{
Cla3phData.u16AdjCnt = 0;
if(((Cla3phData.f32Theta > (Cla3phData.f32GridTheta + MAX_THETA)) &&((Cla3phData.f32Theta < (Cla3phData.f32GridTheta + 3.1415926f)))) ||
((Cla3phData.f32Theta + 3.1415926f) < Cla3phData.f32GridTheta))
{
if(Cla3phData.u16InvPhaseLockCnt)
Cla3phData.u16InvPhaseLockCnt --;
if(Cla3phData.f32Theta > ADJ_THETA)
{
Cla3phData.f32Theta -= ADJ_THETA;
}
}
else if(((Cla3phData.f32GridTheta > (Cla3phData.f32Theta + MAX_THETA)) &&((Cla3phData.f32GridTheta < (Cla3phData.f32Theta + 3.1415926f)))) ||
(Cla3phData.f32GridTheta + 3.1415926f) < (Cla3phData.f32Theta))
{
if(Cla3phData.u16InvPhaseLockCnt)
Cla3phData.u16InvPhaseLockCnt --;
if(Cla3phData.f32Theta < 6.2657412f)
{
Cla3phData.f32Theta += ADJ_THETA;
}
}
else
{
Cla3phData.u16InvPhaseLockCnt ++;
if( Cla3phData.u16InvPhaseLockCnt > INVLOCK_CNT)
{
Cla3phData.u16InvPhaseLockCnt = 0;
Cla3phData.bInvPhaseLocked = true;
}
// if(Cla3phData.u16MainsVCheckRunOneFlag == 1) //只赋值一次
// {
// Cla3phData.u16MainsVCheckRunOneFlag = 0;
// if(Cla3phData.i16Pfc3phStartState >= PfcChargeOn)//V1105_5
// Pll3ph.theta[2] = Cla3phData.f32Theta;
// else
// Pll3ph.theta[2] = 0;
// }
}
}
else
{
if(Cla3phData.u16AdjCnt > 100)
{
Cla3phData.u16AdjCnt = 0;
}
}
}
else
{
if(Cla3phData.i16Pfc3phStartState <= PfcChargeOn)
{
Cla3phData.bInvPhaseLocked = false;
}
// if(Cla3phData.u16MainsVCheckRunOneFlag == 1) //只赋值一次
// {
// Cla3phData.u16MainsVCheckRunOneFlag = 0;
// if(Cla3phData.i16Pfc3phStartState >= PfcChargeOn)
// {
// Pll3ph.theta[1] = Cla3phData.f32Theta;
// }
// else
// {
// Pll3ph.theta[1] = 0;
// }
// }
}
}
// DacbRegs.DACVALS.all = (int16_t)((int16_t)(ClaGridUSpll.sin_theta * 2000 + 2048));
}
/*******************************************************************
* 输入：
* 输出：
* 作用：pfc3phSpllDDSRF：三相锁相
* 说明：相序检测完成之后，进行锁相计算，执行时间:10.8us:
*******************************************************************/
//void CLA_PFC3PHSpllDDSRF(void)
//{
// ClaGridUSpll.cos_theta = (float32)CLAcos(ClaGridUSpll.theta[0]); //cos(theta)
// ClaGridUSpll.sin_theta = (float32)CLAsin(ClaGridUSpll.theta[0]); //sin(theta),超前输入电压90度 +90°
//
// if(true == Cla3phData.i16SeqDoneFlag) //相序计算完成后，计算3phpll
// {
// //*******************正序坐标变换************************//
// if(Sequence_ABC == Cla3phData.i16SeqDir) //正相序
// {
// ClaInvUoutPos.a = Cla3phData.f32UaOutScale;
// ClaInvUoutPos.b = Cla3phData.f32UbOutScale;
// ClaInvUoutPos.c = Cla3phData.f32UcOutScale;
// }
// else if(Sequence_ACB == Cla3phData.i16SeqDir) //负相序,交换输入
// {
// ClaInvUoutPos.a = Cla3phData.f32UaOutScale;
// ClaInvUoutPos.b = Cla3phData.f32UcOutScale;
// ClaInvUoutPos.c = Cla3phData.f32UbOutScale;
// }
//
// ClaInvUoutPos.cosout = ClaGridUSpll.cos_theta;
// ClaInvUoutPos.sinout = ClaGridUSpll.sin_theta;
// ABC_DQ0_POS_CLA_MACRO(ClaInvUoutPos); //dq_p
//
// //*****************负序坐标变换****************************//
// if(Sequence_ABC == Cla3phData.i16SeqDir) //正相序
// {
// ClaInvUoutNeg.a = Cla3phData.f32UaOutScale;
// ClaInvUoutNeg.b = Cla3phData.f32UbOutScale;
// ClaInvUoutNeg.c = Cla3phData.f32UcOutScale;
// }
// else if(Sequence_ACB == Cla3phData.i16SeqDir)//负相序,交换输入
// {
// ClaInvUoutNeg.a = Cla3phData.f32UaOutScale;
// ClaInvUoutNeg.b = Cla3phData.f32UcOutScale;
// ClaInvUoutNeg.c = Cla3phData.f32UbOutScale;
// }
//
// ClaInvUoutNeg.cosout = ClaGridUSpll.cos_theta;
// ClaInvUoutNeg.sinout = ClaGridUSpll.sin_theta;
// ABC_DQ0_NEG_CLA_MACRO(ClaInvUoutNeg); //dq_n
//
// //*****************SPLLDDSRF call**********************//
// ClaGridUSpll.d_p = ClaInvUoutPos.d;
// ClaGridUSpll.q_p = ClaInvUoutPos.q;
// ClaGridUSpll.d_n = ClaInvUoutNeg.d;
// ClaGridUSpll.q_n = ClaInvUoutNeg.q;
// ClaGridUSpll.cos_2theta = (float32)CLAcos((2)*ClaGridUSpll.theta[0]); //CLAsin
// ClaGridUSpll.sin_2theta = (float32)CLAsin((2)*ClaGridUSpll.theta[0]);
// SPLL_3ph_DDSRF_CLA_MACRO(ClaGridUSpll); //锁相出的theta超前输入电压相位90度 Grid+90°
//
// //****正序Uq长时间接近0，认为锁相成功***************//
// if(fabs(ClaInvUoutPos.q) < 0.4)//0.18) //q轴输出保持长时间0值，认为锁相OK,q=0.06
// {
// Cla3phData.u16Pllfailcnt = 0;
// if(Cla3phData.u16PllOkcnt < CLA_TIMER_500MS) //累积500ms
// {
// Cla3phData.u16PllOkcnt++;
// }
// else
// {
// Cla3phData.bPhaseLocked = true; //锁相成功
// }
// }
// else
// {
// Cla3phData.u16PllOkcnt = 0;
// Cla3phData.u16Pllfailcnt++;
// if(Cla3phData.u16Pllfailcnt >= CLA_TIMER_8MS)
// {
// Cla3phData.u16Pllfailcnt = 0;
// Cla3phData.bPhaseLocked = false; //锁相未成功
// }
// }
//
// }/*Cla3phData.i16SeqDoneFlag == true*/
// else //相序检测完成前，复位锁相环参数
// {
// Spll_3ph_DDSRF_Reset(&ClaGridUSpll);
// }
//
//}/*CLA_PFC3PHSpllDDSRF()*/
//
/*******************************************************************
* 输入：
* 输出：
* 作用：pfc3phSpllDDSRF：三相锁相
* 说明：相序检测完成之后，进行锁相计算，执行时间:10.8us:
*******************************************************************/
void CLA_DSOGI3PHPLL(void)
{
float32 f32alpha,f32beta;
if(Cla3phData.u16MainsVLostFlag == 0)//true == Cla3phData.i16SeqDoneFlag)
{ //*******************正序坐标变换************************//
// if(Sequence_ABC == Cla3phData.i16SeqDir) //正相序
// {
ClaInvUoutPos.a = Cla3phData.f32UaOutScale;
ClaInvUoutPos.b = Cla3phData.f32UbOutScale;
ClaInvUoutPos.c = Cla3phData.f32UcOutScale;
// }
// else if(Sequence_ACB == Cla3phData.i16SeqDir) //负相序,交换输入
// {
// ClaInvUoutPos.a = Cla3phData.f32UaOutScale;
// ClaInvUoutPos.b = Cla3phData.f32UcOutScale;
// ClaInvUoutPos.c = Cla3phData.f32UbOutScale;
// }
// else
// {
// ClaInvUoutPos.a = Cla3phData.f32UaOutScale;
// ClaInvUoutPos.b = Cla3phData.f32UbOutScale;
// ClaInvUoutPos.c = Cla3phData.f32UcOutScale;
// }
//***************Grid CLARK transform**************************//
CLARKE_CLA_MACRO(ClaInvUoutPos);
//**************U alpha SOGI************************************//
SPLL_3ph_DSOGI_F_FUNC(&DSogiGridAlpha, ClaInvUoutPos.alpha);
//******************U beta SOGI*********************************//
SPLL_3ph_DSOGI_F_FUNC(&DSogiGridBeta, ClaInvUoutPos.beta);
//******************alpha/beta calculate********************
f32alpha = 0.5 * (DSogiGridAlpha.osg_u[0] - DSogiGridBeta.osg_qu[0]);
f32beta = 0.5 * (DSogiGridAlpha.osg_qu[0] + DSogiGridBeta.osg_u[0]);
//**************park transform & pll******************************//
PARK_SPLL_3PH_F_FUNC(&Pll3ph, f32alpha, f32beta);
if(true == Cla3phData.bInvPhaseLocked)
{
ClaGridUSpll.cos_theta = Pll3ph.cos_out; //cos(theta)
ClaGridUSpll.sin_theta = Pll3ph.sin_out; //sin(theta),超前输入电压90度 +90°
Cla3phData.f32Theta = Pll3ph.theta[2];
}
else
{
if(Pll3ph.fo < 30 ||Pll3ph.fo > 70)
{
ClaGridUSpll.U32PllRstCn ++;
if(ClaGridUSpll.U32PllRstCn > 60000)
{
ClaGridUSpll.U32PllRstCn = 0;
SPLL_3ph_F_Reset(&Pll3ph);
}
}
else
{
if(ClaGridUSpll.U32PllRstCn)
ClaGridUSpll.U32PllRstCn --;
}
}
ClaInvUoutPos.cosout = ClaGridUSpll.cos_theta;
ClaInvUoutPos.sinout = ClaGridUSpll.sin_theta;
ABC_DQ0_POS_CLA_MACRO(ClaInvUoutPos); //dq_p
//****正序Uq长时间接近0，认为锁相成功***************//
if(fabs(ClaInvUoutPos.q) < 0.4)//0.18) //q轴输出保持长时间0值，认为锁相OK,q=0.06
{
Cla3phData.u16Pllfailcnt = 0;
if(Cla3phData.u16PllOkcnt < CLA_TIMER_500MS) //累积500ms
{
Cla3phData.u16PllOkcnt++;
}
else
{
Cla3phData.bPhaseLocked = true; //锁相成功
}
}
else
{
Cla3phData.u16PllOkcnt = 0;
Cla3phData.u16Pllfailcnt++;
if(Cla3phData.u16Pllfailcnt >= CLA_TIMER_8MS)
{
Cla3phData.u16Pllfailcnt = 0;
Cla3phData.bPhaseLocked = false; //锁相未成功
}
}
}/*Cla3phData.i16SeqDoneFlag == true*/
else //相序检测完成前，复位锁相环参数
{
// Spll_3ph_DSOGI_Reset(&DSogiGridAlpha);
// Spll_3ph_DSOGI_Reset(&DSogiGridBeta);
SPLL_3ph_F_Reset(&Pll3ph);
}
}
/*******************************************************************
* 输入：
* 输出：
* 作用：三路电流采样处理
* 执行时间:40k
*******************************************************************/
void ClaAdcTask(void)
{
int16_t temp = 0;
//1路上升下降取平均，2路采MOS
//模块关机下，电流采样作为直流偏置
// if(CpuToCla.i16Pfc3phStartState < PfcDrvOnEn)
// {
// temp = (ADC_B2_IA2 + ADC_B2_IA2_BAKE) >> 1;//ADC_B1_IA2_BAKE;
// ClaAdcInst.f32Ia2Offset = (0.56f * ClaAdcInst.f32Ia2Offset + 0.44f * temp); //5khz
// temp = (ADC_B0_IB2 + ADC_B0_IB2_BAKE) >> 1;//ADC_A0_IA1_BAKE;//实际是IB2
// ClaAdcInst.f32Ib2Offset = (0.56f * ClaAdcInst.f32Ib2Offset + 0.44f * temp); //5khz
// temp = (ADC_ADCIN15_IC2 + ADC_ADCIN15_IC2_BAKE) >> 1;//ADC_ADCIN15_IC2_BAKE;
// ClaAdcInst.f32Ic2Offset = (0.56f * ClaAdcInst.f32Ic2Offset + 0.44f * temp); //5khz
// }
if(CpuToCla.i16Pfc3phStartState < PfcChargeOn && 0 == Cla3phData.FlagADCDcCompFinsh)
{
if(Cla3phData.clacnt > 60)
{
Cla3phData.clacnt = 0;
/*********************************三相逆变2路输出电流Ia，2阶低通滤波,[0.2hz 10hz],滤出直流分量,计算频率1KHZ***************************************/
temp = ADC_A4_IA2;
ClaAdcInst.f32Ia2Offset1 = (0.998743f * ClaAdcInst.f32Ia2Offset1 + 0.001257f * temp); //0.2hz
ClaAdcInst.f32Ia2Offset = (0.937168f * ClaAdcInst.f32Ia2Offset + 0.062832f * ClaAdcInst.f32Ia2Offset1); //10hz
temp = ADC_ADCIN14_IB2;
ClaAdcInst.f32Ib2Offset1 = (0.998743f * ClaAdcInst.f32Ib2Offset1 + 0.001257f * temp); //0.2hz
ClaAdcInst.f32Ib2Offset = (0.937168f * ClaAdcInst.f32Ib2Offset + 0.062832f * ClaAdcInst.f32Ib2Offset1); //10hz
temp = ADC_D2_IC2;
ClaAdcInst.f32Ic2Offset1 = (0.998743f * ClaAdcInst.f32Ic2Offset1 + 0.001257f * temp); //0.2hz
ClaAdcInst.f32Ic2Offset = (0.937168f * ClaAdcInst.f32Ic2Offset + 0.062832f * ClaAdcInst.f32Ic2Offset1); //10hz
}
else
{
Cla3phData.clacnt++;
}
}
else
{
// if(ClaAdcInst.f32Ia2Offset1 > ADC_OFFSETMAX)
// ClaAdcInst.f32Ia2Offset1 = ADC_OFFSETMAX;
// else if(ClaAdcInst.f32Ia2Offset1 < ADC_OFFSETMIN)
// ClaAdcInst.f32Ia2Offset1 = ADC_OFFSETMIN;
//
// if(ClaAdcInst.f32Ib2Offset1 > ADC_OFFSETMAX)
// ClaAdcInst.f32Ib2Offset1 = ADC_OFFSETMAX;
// else if(ClaAdcInst.f32Ib2Offset1 < ADC_OFFSETMIN)
// ClaAdcInst.f32Ib2Offset1 = ADC_OFFSETMIN;
//
// if(ClaAdcInst.f32Ic2Offset1 > ADC_OFFSETMAX)
// ClaAdcInst.f32Ic2Offset1 = ADC_OFFSETMAX;
// else if(ClaAdcInst.f32Ic2Offset1 < ADC_OFFSETMIN)
// ClaAdcInst.f32Ic2Offset1 = ADC_OFFSETMIN;
}
//************电流方向取反，对照硬件采样电位 10.31************************//
//三相逆变输出电流Ia2
ClaAdcInst.f32Ia2 = ADC_A4_IA2 - ClaAdcInst.f32Ia2Offset;
//三相逆变输出电流Ib2
ClaAdcInst.f32Ib2 = ADC_ADCIN14_IB2 - ClaAdcInst.f32Ib2Offset;
//三相逆变输出电流Ic2
ClaAdcInst.f32Ic2 = ADC_D2_IC2 - ClaAdcInst.f32Ic2Offset;
//经过校准后的实际电流
ClaAdcRaw.f32Ia2 = (float32)ClaAdcInst.f32Ia2 * GRID_IA_SCALE;
ClaAdcRaw.f32Ib2 = (float32)ClaAdcInst.f32Ib2 * GRID_IB_SCALE;
ClaAdcRaw.f32Ic2 = (float32)ClaAdcInst.f32Ic2 * GRID_IC_SCALE;
/****************LP*******************************************/
// ClaToCpu.f32I2AbsMax = MAX(fabs(ClaAdcRaw.f32Ia2),fabs(ClaAdcRaw.f32Ib2));
// ClaToCpu.f32I2AbsMax = MAX(fabs(ClaAdcRaw.f32Ic2),ClaToCpu.f32I2AbsMax);
ClaToCpu.Grid_PosPower2_Ud = (Pll3ph.u_D[0]*4194304*Pll3ph.u_D[0]);///
ClaToCpu.Grid_PosPower2_Uq = (Pll3ph.u_Q[0]*4194304*Pll3ph.u_Q[0]);///
ClaToCpu.Grid_PosPower2_LP100HzUd = LowPassFilter( ClaToCpu.Grid_PosPower2_Ud,LPF_100HZ_60K_RUN, ClaToCpu.Grid_PosPower2_LP100HzUd);
ClaToCpu.Grid_PosPower2_LP100HzUq = LowPassFilter( ClaToCpu.Grid_PosPower2_Uq,LPF_100HZ_60K_RUN, ClaToCpu.Grid_PosPower2_LP100HzUq);
ClaToCpu.f32Pos_Id = ClaIacDQ.d;
ClaToCpu.f32Pos_Iq = ClaIacDQ.q;
ClaToCpu.f32Pos_LP100HzId = LowPassFilter( ClaToCpu.f32Pos_Id,LPF_100HZ_60K_RUN, ClaToCpu.f32Pos_LP100HzId);
ClaToCpu.f32Pos_LP100HzIq = LowPassFilter( ClaToCpu.f32Pos_Iq,LPF_100HZ_60K_RUN, ClaToCpu.f32Pos_LP100HzIq);
}/*ClaAdcTask()*/
/**************************************************************************************
* 输入：
* 输出：
* 作用：cla中环路控制算法：2路电流控制
**************************************************************************************/
void ClaControlLoop(ADCRAW_CLA *I)
{
if(1 == CpuToCla.u16PwmOnTest)
{
ClaLoopReset();
// ClaSpwm3ph.a = CpuToCla.a;
// ClaSpwm3ph.b = CpuToCla.b;
// ClaSpwm3ph.c = CpuToCla.c;
// ClaPfc3phPwmDutyUpdate(&ClaSpwm3ph);
// Pfc3phSlaverPwmEnable();
return;
}
// if(true == Cla3phData.i16SeqDoneFlag) //相序计算完成后
// {
//*******************正序坐标变换************************//
// if(Sequence_ABC == Cla3phData.i16SeqDir) //正相序
// {
ClaIacDQ.a = I->f32Ia2;
ClaIacDQ.b = I->f32Ib2;
ClaIacDQ.c = I->f32Ic2;
// }
// else if(Sequence_ACB == Cla3phData.i16SeqDir) //负相序,交换输入
// {
// ClaIacDQ.a = I->f32Ia2;
// ClaIacDQ.b = I->f32Ic2;
// ClaIacDQ.c = I->f32Ib2;
// }
ClaIacDQ.cosout = ClaGridUSpll.cos_theta;
ClaIacDQ.sinout = ClaGridUSpll.sin_theta;
ABC_DQ0_POS_CLA_MACRO(ClaIacDQ); //ABC-->DQ
//从路开波使能
// if(CpuToCla.u16SlvDrv == DrvSlaveOn)
// {
// //预开波
//// ClaSpwm3ph.a = 0;
//// ClaSpwm3ph.b = -0.5 * Cla_Sqrt3;
//// ClaSpwm3ph.c = 0.5 * Cla_Sqrt3;
//// ClaPfc3phPwmDutyUpdate(&ClaSpwm3ph);
// }
// else
if(CpuToCla.i16Pfc3phStartState >= PfcChargeOn)
{
//D轴电流环：电压环输出一半
// ClaPRId1.Vars.Ref = ClaId.Vars.Ref;
// ClaPRcontrol(&ClaPRId1, ClaIacDQ.d);
//
// ClaPRIq1.Vars.Ref = 0;
// ClaPRcontrol(&ClaPRIq1, ClaIacDQ.q);
if(true == Cla3phData.PR_ParaVaild && 1 == PWMEnableFlagCLA)
{
Cla3phData.PR_ParaIndex = PR_AllFreq;
switch( Cla3phData.PR_ParaIndex)
{
case PR_CC1Freq:
ClaVdLoopPR_4Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //4倍频
ClaPRcontrol(&ClaVdLoopPR_4Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_4Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_4Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_CC2Freq;
break;
case PR_CC2Freq:
ClaVdLoopPR_6Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //6倍频
ClaPRcontrol(&ClaVdLoopPR_6Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_6Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_6Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_CC3Freq;
break;
case PR_CC3Freq:
// ClaVdLoopPR_12Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //12倍频
// ClaPRcontrol(&ClaVdLoopPR_12Freq,Cla3phData.f32InvUd_Pos);
// ClaVqLoopPR_12Freq.Vars.Ref = 0;
// ClaPRcontrol(&ClaVqLoopPR_12Freq,Cla3phData.f32InvUq_Pos);
ClaVdLoopPR_4Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //4倍频
ClaPRcontrol(&ClaVdLoopPR_4Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_4Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_4Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_CC4Freq;
break;
case PR_CC4Freq:
// ClaVdLoopPR_18Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //18倍频
// ClaPRcontrol(&ClaVdLoopPR_18Freq,Cla3phData.f32InvUd_Pos);
// ClaVqLoopPR_18Freq.Vars.Ref = 0;
// ClaPRcontrol(&ClaVqLoopPR_18Freq,Cla3phData.f32InvUq_Pos);
ClaVdLoopPR_6Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //6倍频
ClaPRcontrol(&ClaVdLoopPR_6Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_6Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_6Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_CC1Freq;
break;
case PR_AllFreq:
ClaVdLoopPR_4Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //4倍频
ClaPRcontrol(&ClaVdLoopPR_4Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_4Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_4Freq,Cla3phData.f32InvUq_Pos);
ClaVdLoopPR_6Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //6倍频
ClaPRcontrol(&ClaVdLoopPR_6Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_6Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_6Freq,Cla3phData.f32InvUq_Pos);
// ClaVdLoopPR_18Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //18倍频
// ClaPRcontrol(&ClaVdLoopPR_18Freq,Cla3phData.f32InvUd_Pos);
// ClaVqLoopPR_18Freq.Vars.Ref = 0;
// ClaPRcontrol(&ClaVqLoopPR_18Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_AllFreq;
break;
case PR_One4Freq:
ClaVdLoopPR_4Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //4倍频
ClaPRcontrol(&ClaVdLoopPR_4Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_4Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_4Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_One4Freq;
break;
case PR_One6Freq:
ClaVdLoopPR_6Freq.Vars.Ref = Cla3phData.f32InvUd_Pos_Ref; //6倍频
ClaPRcontrol(&ClaVdLoopPR_6Freq,Cla3phData.f32InvUd_Pos);
ClaVqLoopPR_6Freq.Vars.Ref = 0;
ClaPRcontrol(&ClaVqLoopPR_6Freq,Cla3phData.f32InvUq_Pos);
Cla3phData.PR_ParaIndex = PR_One6Freq;
break;
default:
Cla3phData.PR_ParaIndex = PR_AllFreq;
break;
}
}
else
{
if(true == Cla3phData.PR_ParaErrFlag)
{
//ClaVdLoopPR_2Freq.Vars.Output = 0;
ClaVdLoopPR_4Freq.Vars.Output = 0;
//ClaVdLoopPR_8Freq.Vars.Output = 0;
ClaVdLoopPR_6Freq.Vars.Output = 0;
// ClaVdLoopPR_12Freq.Vars.Output = 0;
//ClaVdLoopPR_18Freq.Vars.Output = 0;
//ClaVqLoopPR_2Freq.Vars.Output = 0;
ClaVqLoopPR_4Freq.Vars.Output = 0;
//ClaVqLoopPR_8Freq.Vars.Output = 0;
ClaVqLoopPR_6Freq.Vars.Output = 0;
// ClaVqLoopPR_12Freq.Vars.Output = 0;
//ClaVqLoopPR_18Freq.Vars.Output = 0;
}
}
Cla3phData.f32InvUdPRloopout = ClaVdLoopPR_4Freq.Vars.Output + ClaVdLoopPR_6Freq.Vars.Output;
Cla3phData.f32InvUqPRloopout = ClaVqLoopPR_4Freq.Vars.Output + ClaVqLoopPR_6Freq.Vars.Output;
ClaId.Vars.Ref = Cla3phData.f32InvUdloopout * 0.5f;
ClaPosiPiOnly(&ClaId, ClaIacDQ.d);
//Q轴电流环:q轴电流给定(无功功率)是0
ClaIq.Vars.Ref = Cla3phData.f32InvUqloopout * 0.5f; //
ClaPosiPiOnly(&ClaIq, ClaIacDQ.q);
//PR电流环1
// Cla_PR_Para_Cal(&ClaPRId1, 300, DELTA_TS, ClaPRLoopId1.Wc, ClaPRLoopId1.Kr, ClaPRLoopId1.Kp);
// Cla_PR_Para_Cal(&ClaPRIq1, 300, DELTA_TS, ClaPRLoopIq1.Wc, ClaPRLoopIq1.Kr, ClaPRLoopIq1.Kp);
// ClaPRId1.Vars.Ref = ClaId.Vars.Ref;
// ClaPRcontrol(&ClaPRId1, ClaIacDQ.d);
//
// ClaPRIq1.Vars.Ref = 0;
// ClaPRcontrol(&ClaPRIq1, ClaIacDQ.q);
//环路输出结果控制的是SR驱动，所以针对MOS来说，环路输出需要取反！！
//首次开波，MOS需要从小duty发波，即环路输出是赋值最大
ClaSpwm3ph.d = ClaId.Vars.Output + CpuToCla.f32Ed;// * 0.6f;// * ClaToCpu.f32LoopCoeff;
ClaSpwm3ph.q = ClaIq.Vars.Output + CpuToCla.f32Eq;// * 0.6f;// * ClaToCpu.f32LoopCoeff;
ClaSpwm3ph.z = 0;
// ClaToCpu.f32LoopCoeff -= 0.000005;
// if(ClaToCpu.f32LoopCoeff < 0)
// {
// ClaToCpu.f32LoopCoeff = 0;
// }
//dq-->abc
// ClaSpwm3ph.cos_out = ClaGridUSpll.cos_theta;
// ClaSpwm3ph.sin_out = ClaGridUSpll.sin_theta;
ClaSpwm3ph.cos_out = ClaIacDQ.cosout;
ClaSpwm3ph.sin_out = ClaIacDQ.sinout;
DQ0_ABC_CLA_MACRO(ClaSpwm3ph);
//占空比更新
ClaPfc3phPwmDutyUpdate(&ClaSpwm3ph);
// //校准模式下，1路2路分别发波校准电流
// if(CpuToCla.u16DebugMode == 1)
// {
// return;
// }
//交错路pwm开波
if(PWMEnableFlagCLA == 0)
{
Pfc3phSlaverPwmEnable();
PWMEnableFlagCLA = 1;
}
// DacbRegs.DACVALS.all = (int16_t)((int16_t)(ClaIacDQ.sinout * 2000 + 2048));//IdLoop.Vars.Output
} /*CpuToCla.i16Pfc3phStartState >= PfcChargeOn*/
else
{
//封波处理
Cla3phData.f32InvUdPRloopout = 0;
Cla3phData.f32InvUqPRloopout = 0;
PWMEnableFlagCLA = 0;
Pfc3phSlaverPwmDisable();
//环路参数复位
ClaLoopReset();
}
// }/*Cla3phData.i16SeqDoneFlag == true*/
// else
// {
// //封波处理
// Pfc3phSlaverPwmDisable();
// }
}/*ClaControlLoop*/
/*********************************************************************
* 输入：
* 输出：
* 作用：3 phase pfc pwm update
* 说明：3.3us
* A2相/B2相/C2相
*---------------------------------------------------
* |
* |
* PWM7A/TA1(A2相)
* PWM9A/TB1(B2相)---互补---\
* PWM11A/TC1(C2相) \
* | \
* | PWM7B/TA3 PWM8B/TA4
* |-------------- PWM9B/TB3----PWM10B/TB4------------
* | PWM11B/TC3 PWM12B/TC4
* | /
* PWM8A/TA2 /
* PWM10A/TB2---------互补------------/
* PWM12A/TC2
* |
* |
*-------------------------------------------------------
* ********************************************************************/
void ClaPfc3phPwmDutyUpdate(DQ0_ABC_CLA *v)
{
float32 f32DutyTempA = 0;
float32 f32DutyTempB = 0;
float32 f32DutyTempC = 0;
//*********根据正负序，决定PWM驱动顺序*****************//
// if(Sequence_ABC == Cla3phData.i16SeqDir) //正相序
// {
Cla3phLoopOut.f32a = v->a + CpuToCla.f32HarmInj;// - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj; //偏压
Cla3phLoopOut.f32b = v->b + CpuToCla.f32HarmInj;// - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj;
Cla3phLoopOut.f32c = v->c + CpuToCla.f32HarmInj;// - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj;
// }
// else if(Sequence_ACB == Cla3phData.i16SeqDir) //负相序
// {
// Cla3phLoopOut.f32a = v->a - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj; //偏压
// Cla3phLoopOut.f32b = v->c - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj;
// Cla3phLoopOut.f32c = v->b - CpuToCla.f32Vdifflpout + CpuToCla.f32HarmInj;
// }
//****************spwm调制方式*****************//
//****************spwm调制方式*****************//
//A相防过零抖动???
if(Cla3phLoopOut.f32a > 0)
{
f32DutyTempA = Cla3phLoopOut.f32a + 0.01f;
}
else if(Cla3phLoopOut.f32a < 0)
{
f32DutyTempA = -Cla3phLoopOut.f32a + 0.01f;
}
else
{
f32DutyTempA = 0;
}
//限幅：
#ifdef MATLAB_SIM
f32DutyTempA = CLA_MIN(f32DutyTempA, Cla3phLoopOut.f32limitMax);
f32DutyTempA = CLA_MAX(f32DutyTempA, 0.0);
#else
f32DutyTempA = __mminf32(f32DutyTempA, Cla3phLoopOut.f32limitMax);
f32DutyTempA = __mmaxf32(f32DutyTempA, 0.0);
#endif
//B相防过零抖动???
if(Cla3phLoopOut.f32b > 0)
{
f32DutyTempB = Cla3phLoopOut.f32b + 0.01f;
}
else if(Cla3phLoopOut.f32b < 0)
{
f32DutyTempB = -Cla3phLoopOut.f32b + 0.01f;
}
else
{
f32DutyTempB = 0;
}
//限幅
#ifdef MATLAB_SIM
f32DutyTempB = CLA_MIN(f32DutyTempB, Cla3phLoopOut.f32limitMax);
f32DutyTempB = CLA_MAX(f32DutyTempB, 0.0);
#else
f32DutyTempB = __mminf32(f32DutyTempB, Cla3phLoopOut.f32limitMax);
f32DutyTempB = __mmaxf32(f32DutyTempB, 0.0);
#endif
//C相防过零抖动???
if(Cla3phLoopOut.f32c > 0)
{
f32DutyTempC = Cla3phLoopOut.f32c + 0.01f;
}
else if(Cla3phLoopOut.f32c < 0)
{
f32DutyTempC = -Cla3phLoopOut.f32c + 0.01f;
}
else
{
f32DutyTempC = 0;
}
//限幅:不会完全直通
#ifdef MATLAB_SIM
f32DutyTempC = CLA_MIN(f32DutyTempC, Cla3phLoopOut.f32limitMax);
f32DutyTempC = CLA_MAX(f32DutyTempC, 0.0);
#else
f32DutyTempC = __mminf32(f32DutyTempC, Cla3phLoopOut.f32limitMax);
f32DutyTempC = __mmaxf32(f32DutyTempC, 0.0);
#endif
//*************250us,限值放开**********//
Cla3phLoopOut.f32limitMax += 0.02;
if(Cla3phLoopOut.f32limitMax >= 0.97) //1.0 //0.99
{
Cla3phLoopOut.f32limitMax = 0.97;
}
#ifndef MATLAB_SIM
//**********************A相发波********************************//
if(Cla3phLoopOut.f32a > 0) //正半周，TA2低电平，TA1调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TA2低电平
// EPwm8Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //8A--TA2
// EPwm8Regs.CMPA.bit.CMPA = EPwm8Regs.TBPRD - 5;
EPwm8Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //8A--TA2
EPwm8Regs.CMPA.bit.CMPA = 5;
//TA1调制占空比
EPwm7Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //7A--TA1
// if(f32DutyTempA == 0)
// {
// EPwm7Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //7A--TA1
// }
// else if(f32DutyTempA == 1)//允许常高吗？？
// {
// EPwm7Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; // 7A--TA1
// }
EPwm7Regs.CMPA.bit.CMPA = (uint16_t)((1 - f32DutyTempA) * EPwm7Regs.TBPRD);
}
else //负半周，TA1低电平，TA2调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TA1低电平
EPwm7Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //7A--TA1
EPwm7Regs.CMPA.bit.CMPA = EPwm7Regs.TBPRD - 5;
//TA2调制占空比6
EPwm8Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //8A--TA2
// if(f32DutyTempA == 0)
// {
// EPwm8Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //8A--TA2
// }
// else if(f32DutyTempA == 1)//允许常高吗？？
// {
// EPwm8Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //8A--TA2
// }
EPwm8Regs.CMPA.bit.CMPA = (uint16_t)(f32DutyTempA * EPwm8Regs.TBPRD);
}
//**********************B相发波********************************//
if(Cla3phLoopOut.f32b > 0) //正半周，TB2低电平，TB1调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TB2低电平
// EPwm10Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //10A--TB2
// EPwm10Regs.CMPA.bit.CMPA = EPwm10Regs.TBPRD - 5;
EPwm10Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //10A--TB2
EPwm10Regs.CMPA.bit.CMPA = 5;
//TB1调制占空比
EPwm9Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //9A--TB1
// if(f32DutyTempB == 0)
// {
// EPwm9Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //9A--TB1
// }
// else if(f32DutyTempB == 1)//允许常高吗？？
// {
// EPwm9Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; // 9A--TB1
// }
EPwm9Regs.CMPA.bit.CMPA = (uint16_t)((1 - f32DutyTempB) * EPwm9Regs.TBPRD);
}
else //负半周，TB1低电平，TB2调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TB1低电平
EPwm9Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //9A--TB1
EPwm9Regs.CMPA.bit.CMPA = EPwm9Regs.TBPRD - 5;
//TB2调制占空比
EPwm10Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //10A--TB2
// if(f32DutyTempB == 0)
// {
// EPwm10Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //10A--TB2
// }
// else if(f32DutyTempB == 1)//允许常高吗？？
// {
// EPwm10Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //10A--TB2
// }
EPwm10Regs.CMPA.bit.CMPA = (uint16_t)(f32DutyTempB * EPwm10Regs.TBPRD);
}
//**********************C相发波********************************//
if(Cla3phLoopOut.f32c > 0) //正半周，TC2低电平，TC1调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TC2低电平
// EPwm12Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //12A--TC2
// EPwm12Regs.CMPA.bit.CMPA = EPwm12Regs.TBPRD - 5;
EPwm12Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //12A--TC2
EPwm12Regs.CMPA.bit.CMPA = 5;
//TC1调制占空比
EPwm11Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //11A--TC1
// if(f32DutyTempC == 0)
// {
// EPwm11Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //11A--TC1
// }
// else if(f32DutyTempC == 1)//允许常高吗？？
// {
// EPwm11Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //11A--TC1
// }
EPwm11Regs.CMPA.bit.CMPA = (uint16_t)((1 - f32DutyTempC) * EPwm11Regs.TBPRD);
}
else //负半周，TC1低电平，TC2调制占空比
{
//****交错路发波：通过环路参数互补发波180度*******//
//TC1低电平
EPwm11Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //11A--TC1
EPwm11Regs.CMPA.bit.CMPA = EPwm11Regs.TBPRD - 5;
//TC2调制占空比
EPwm12Regs.AQCSFRC.bit.CSFA = CSFA_DISABLE; //12A--TC2
// if(f32DutyTempC == 0)
// {
// EPwm12Regs.AQCSFRC.bit.CSFA = CSFA_LOW; //12A--TC2
// }
// else if(f32DutyTempC == 1)//允许常高吗？？
// {
// EPwm12Regs.AQCSFRC.bit.CSFA = CSFA_HIGH; //12A--TC2
// }
EPwm12Regs.CMPA.bit.CMPA = (uint16_t)(f32DutyTempC * EPwm12Regs.TBPRD);
}
#endif
}/*pfc3phPwmDutyUpdate*/
/*******************************************************************
* 输入：
* 输出：
* 作用：计算PQ以及PLL算法
* 执行时间: 40K,PWMINT触发
* 实测8us--28374s
*******************************************************************/
#pragma CODE_SECTION(AlgorithmCla, "Cla1Prog");
interrupt void AlgorithmCla(void)
{
// CBC_FLAG_GPIO55_ON;//TODO
ClaReadCpuData(); //从cpu读取数据
ClaMainsVoltCheck(); //市电失电检测
// CLA_PFC3PHSpllDDSRF(); //锁相算法
CLA_DSOGI3PHPLL(); //三相DSOGI
ClaAdcTask(); //adc采样处理
ClaSendCpuData(); //传递数据至cpu
ClaControlLoop(&ClaAdcRaw); //环路控制
// CBC_FLAG_GPIO55_OFF;//TODO
}/*AlgorithmCla*/
最新发布
理解命令执行等级：CLA详解
本文深入探讨了命令执行等级（CLA），解释了不同应用指令的分类与用途，为理解命令执行流程提供了清晰的指引。
扫一扫
1415
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
