/************************************************************************ Project: Welling Motor Control Paltform Filename: spdctrmode.c Partner Filename: spdctrmode.h Description: Speed control mode Complier: IAR Embedded Workbench for ARM 7.80, IAR Systems. CPU TYPE : GD32F3x0 ************************************************************************* Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd. All rights reserved. ************************************************************************* ************************************************************************* Revising History (ECL of this file): ************************************************************************/ /************************************************************************ Beginning of File, do not put anything above here except notes Compiler Directives: *************************************************************************/ #ifndef _SPDCTRMODE_C_ #define _SPDCTRMODE_C_ #endif /************************************************************************ Included File *************************************************************************/ #include "syspar.h" #include "user.h" #include "bikeinformation.h" #include "FuncLayerAPI.h" #include "AssistCurve.h" #include "cmdgennew.h" #include "CodePara.h" #include "CadAssist.h" //#include "api.h" #include "TimeTask_Event.h" #include "emcdeal.h" #include "LoadObsTheta.h" #include "obs.h" #include "FSM_1st.h" #include "cmdgennew.h" /************************************************************************ Constant Table (N/A) *************************************************************************/ /************************************************************************* Exported Functions (N/A) *************************************************************************/ /*************************************************************** Function: scm_voSpdCtrMdInit; Description: Speed control mode initializing function Call by: rmd_voModeSchd(); Input Variables: N/A Output/Return Variables: N/A Subroutine Call: ...; Reference: N/A ****************************************************************/ void scm_voSpdCtrMdInit(void) { // /* PWM init */ // hw_voPWMInit(); /*cmd handle Initial */ cmd_voCmdInit(); /* Current PI init */ acr_voCurPIInit(); /* Current decoupling init */ acr_voUdqDcpInit(); /* Sensorless observer init */ obs_voObsInit(); /* SPI position sensor init */ //spi_voResolverInit(); /* Speed PI init */ asr_voSpdPIInit(); // asrnew_voSpdPIInit(); torqobs_voInit(); // LoadObsTheta_voInit(); /* Flux weakening init */ #if(FLUX_MODE == 0) spdflx_voInit(); #elif(FLUX_MODE == 1) flx_voInit(); #else //Config Error #endif // fw_voInit(); /* Power Limit init */ pwr_voPwrLimInit(); /* SVPWM init */ pwm_voInit(); /* Dead time init */ dbc_voDBCompInit(); /* Contant voltage brake init */ cvb_voBrakeInit(); /* switchHall init */ switchhall_voInit(); /* Align pos startup open2clz clzloop init */ align_voInit(); scm_stSpdFbkLpf.slY.sw.hi = 0; scm_swSpdRefPu = 0; scm_swUalphaPu = 0; // Q14 scm_swUbetaPu = 0; // Q14 scm_stIdFbkLpf.slY.sl = 0; // Id feedback LPF scm_stIqFbkLpf.slY.sl = 0; // Iq feedback LPF scm_stIqRefforDesat.slY.sl = 0; // Iq Ref for desaturation Lpf scm_stIqFbkforDesat.slY.sl = 0; // Iq Fbk for desaturation Lpf scm_swIdRefPu = 0; // Q14 scm_swIqRefPu = 0; // Q14 scm_uwAngRefPu = 0; // Q15 scm_uwAngParkPu = 0; // Q15 scm_uwAngIParkPu = 0; // Q15 scm_swRotateDir = -1; // Direction of motor rotate scm_ulStatCt = 0; // Status hold time count scm_uwAngManuPu = 0; // Q15, Angle given manually scm_slAngManuPu = 0; scm_slDragSpdPu = 0; // Q15, Drag speed scm_slDragSpdRefPu = 0; // Q29, intermediate Drag speed scm_blCurSwitchOvrFlg = FALSE; // Current switch over flag scm_blAngSwitchOvrFlg = FALSE; // Angle switch over flag scm_uwAngSwitchK = 0; // Angle switch weight value scm_swMotorPwrInWt = 0; // unit: w, Input power of motor scm_blCoefUpdateFlg = FALSE; // Coefficient update flag scm_stIqLoadLpf.slY.sl = 0; scm_stSpdFbkLpf.slY.sl = 0; // Speed feedback LPF scm_uwSpdFbkLpfAbsPu = 0; // Q15, Speed feedback LPF absolute scm_swMotorPwrInPu = 0; // Q15, Input power of motor scm_swSystemPwrInPu = 0; // Q15, Input power of System scm_stMotoPwrInLpf.slY.sl = 0; // Input power of motor after LPF scm_stPCBTempLpf.slY.sl = 0; scm_stMotorTempLpf.slY.sl = 0; scm_swMotorPwrInLpfWt = 0; // unit: 0.1w, Input power of motor after LPF scm_uwMotorPwrInAvgPu = 0; // Q15, Input power of motor after average filter scm_swIdFdbLpfPu = 0; scm_swIqFdbLpfPu = 0; scm_swUdRefPu = 0; scm_swUqRefPu = 0; scm_swUalphaFbkPu = 0; scm_swUbetaFbkPu = 0; scm_swUalphaRefPu = 0; scm_swUbetaRefPu = 0; scm_swUalphaCompPu = 0; scm_swUbetaCompPu = 0; scm_uwHfiAngZ1Pu = 0; scm_slAngSumPu = 0; scm_slAngErrPu = 0; scm_blAngSumOvrFlg = FALSE; scm_uwRunMdSw = 1; scm_ulRunMdSwCt = 0; scm_ulCloseCt = 0; scm_uwStartMd = cp_stControlPara.swStartMode; scm_uwStartMdSw = scm_uwStartMd; scm_uwInitPosMd = cp_stControlPara.swInitPosMode; scm_uwInitPosMdSw = scm_uwInitPosMd; scm_uwHfiOvrCnt = 0; scm_slIdRefPu = 0; } /*************************************************************** Function: scm_voSpdCtrMdCoef; Description: Speed control mode TBS scheduler Call by: tbs_voIsr(); Input Variables: N/A Output/Return Variables: N/A Subroutine Call: ...; Reference: N/A ****************************************************************/ void scm_voSpdCtrMdCoef(void) { ULONG ulLpfTm; // unit: us UWORD uwLqPu = 0; ULONG ulAccel100rpmpsPu = USER_MOTOR_75RPMPS2PU_Q29;//USER_MOTOR_240RPMPS2PU_Q29; if (abs(scm_swIqRefPu) < mn_swIqTurn1Pu) { scm_uwLqPu = cof_uwLqPu; } else { uwLqPu = mn_slLqTurn1Pu + ((SLONG)(abs(scm_swIqRefPu) - mn_swIqTurn1Pu) * mn_swKLqSat >> 10); // Q10 if (uwLqPu < cof_uwLqMinPu) { scm_uwLqPu = cof_uwLqMinPu; } else if (uwLqPu > cof_uwLqPu) { scm_uwLqPu = cof_uwLqPu; } else { scm_uwLqPu = uwLqPu; } } /* Sensorless observer coefficient calculate */ obs_stObsCoefIn.uwRbOm = cof_uwRbOm; // Real Value, unit: 0.01Ohm, Resistance base obs_stObsCoefIn.uwLbHm = cof_uwLbHm; // Real Value, unit: 0.01mH, Inductance base obs_stObsCoefIn.uwFluxbWb = cof_uwFluxbWb; // Real Value, unit: 0.01mWb, Flux linkage base obs_stObsCoefIn.uwFbHz = cof_uwFbHz; // Real Value, Unit:Hz frequency base obs_stObsCoefIn.uwRsOm = cp_stMotorPara.swRsOhm; // Real Value, unit: 0.01Ohm, Resistance base obs_stObsCoefIn.uwLqHm = ((ULONG)scm_uwLqPu * cof_uwLbHm) >> 10; // Real Value, unit: 0.01mH, q Inductance obs_stObsCoefIn.uwLdHm = cp_stMotorPara.uwLdmH; // Real Value, unit: 0.01mH, d Inductance obs_stObsCoefIn.uwFluxWb = cp_stMotorPara.swFluxWb; // Real Value, unit: 0.01mWb, Flux linkage obs_stObsCoefIn.uwFreqTbcHz = FTBC_HZ; // Real Value, Unit:Hz Tbc obs_stObsCoefIn.uwFluxDampingRatio = cp_stControlPara.swObsFluxPIDampratio; // Real Value, unit:0.1 obs_stObsCoefIn.uwFluxCrossFreqHz = cp_stControlPara.swObsFluxPICrossfreHz; // Real Value, unit:Hz obs_stObsCoefIn.uwSpdPllWvcHz = cp_stControlPara.swObsSpdPLLBandWidthHz; // Real Value, Unit:Hz obs_stObsCoefIn.uwSpdPllMcoef = cp_stControlPara.swObsSpdPLLM; obs_voObsCoef(&obs_stObsCoefIn, &obs_stObsCoefPu); /* Speed PI coefficient calculate */ asr_stSpdPICoefIn.uwUbVt = VBASE; asr_stSpdPICoefIn.uwIbAp = IBASE; asr_stSpdPICoefIn.uwFbHz = FBASE; asr_stSpdPICoefIn.uwFTbsHz = FTBS_HZ; asr_stSpdPICoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs; asr_stSpdPICoefIn.uwMtJm = cp_stMotorPara.swJD; asr_stSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb; asr_stSpdPICoefIn.uwMcoef = cp_stControlPara.swAsrPIM; asr_stSpdPICoefIn.uwWvcHz = cp_stControlPara.swAsrPIBandwidth; asr_stSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate; asr_voSpdPICoef(&asr_stSpdPICoefIn, &asr_stSpdPICoef); /* 拖拽参数初始化 */ align_voCoef(); /* Reduced Order Torque Observe coefficient calculate */ torqobs_stCoefIn.uwUbVt = VBASE; torqobs_stCoefIn.uwIbAp = IBASE; torqobs_stCoefIn.uwFbHz = FBASE; torqobs_stCoefIn.uwFTbsHz = FTBS_HZ; torqobs_stCoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs; torqobs_stCoefIn.uwMtJm = cp_stMotorPara.swJD << 2; // cp_stMotorPara.swJD; torqobs_stCoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb; torqobs_stCoefIn.uwWtcHz = 50; // cp_stControlPara.swAsrPIBandwidth; torqobs_stCoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate; torqobs_voCoef(&torqobs_stCoefIn, &torqobs_stCoef); mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &scm_stIqLoadLpf.uwKx); // 50Hz /* Full Order Torque Observer coefficient calculate */ // LoadObsTheta_stCoefIn.uwFbHz = FBASE; // LoadObsTheta_stCoefIn.uwFluxbWb = cof_uwFluxbWb; // LoadObsTheta_stCoefIn.uwFluxWb = cp_stMotorPara.swFluxWb; // LoadObsTheta_stCoefIn.uwFTbcHz = FTBC_HZ; // LoadObsTheta_stCoefIn.uwJb = cof_uwJb; // LoadObsTheta_stCoefIn.uwMtJm = cp_stMotorPara.swJD << 2; // LoadObsTheta_stCoefIn.uwWtcHz = 200; // LoadObsTheta_stCoefIn.uwMCoef = 100; // LoadObsTheta_voCoef(); /* Id PI coefficient calculate */ acr_stCurIdPICoefIn.uwFbHz = FBASE; acr_stCurIdPICoefIn.uwUbVt = VBASE; acr_stCurIdPICoefIn.uwIbAp = IBASE; acr_stCurIdPICoefIn.uwLHm = cp_stMotorPara.uwLdmH; acr_stCurIdPICoefIn.uwMtRsOh = cp_stMotorPara.swRsOhm; acr_stCurIdPICoefIn.uwFTbcHz = FTBC_HZ; acr_stCurIdPICoefIn.uwRaCoef = cp_stControlPara.swAcrRaCoef; // Coefficient of Active Resistance acr_stCurIdPICoefIn.uwWicHz = cp_stControlPara.swAcrPIBandwidth; // Current loop frequency bandwidth acr_voCurPICoef(&acr_stCurIdPICoefIn, &acr_stCurIdPICoef); /* Iq PI coefficient calculate */ acr_stCurIqPICoefIn.uwFbHz = FBASE; acr_stCurIqPICoefIn.uwUbVt = VBASE; acr_stCurIqPICoefIn.uwIbAp = IBASE; acr_stCurIqPICoefIn.uwLHm = cp_stMotorPara.uwLqmH; acr_stCurIqPICoefIn.uwMtRsOh = cp_stMotorPara.swRsOhm; acr_stCurIqPICoefIn.uwFTbcHz = FTBC_HZ; acr_stCurIqPICoefIn.uwRaCoef = cp_stControlPara.swAcrRaCoef; acr_stCurIqPICoefIn.uwWicHz = cp_stControlPara.swAcrPIBandwidth; acr_voCurPICoef(&acr_stCurIqPICoefIn, &acr_stCurIqPICoef); /* Current decoupling coefficient calculate */ acr_stUdqDcpCoefIn.uwLdHm = cp_stMotorPara.uwLdmH; acr_stUdqDcpCoefIn.uwLqHm = cp_stMotorPara.uwLqmH; acr_stUdqDcpCoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb; acr_stUdqDcpCoefIn.uwUbVt = VBASE; acr_stUdqDcpCoefIn.uwFbHz = FBASE; acr_stUdqDcpCoefIn.uwIbAp = IBASE; acr_voUdqDcpCoef(&acr_stUdqDcpCoefIn, &acr_stUdqDcpCoef); /* Id feedback low pass filter coef */ ulLpfTm = 1000000 / cp_stControlPara.swAcrCurFbLpfFre; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIdFbkLpf.uwKx); /* Iq feedback low pass filter coef */ ulLpfTm = 1000000 / cp_stControlPara.swAcrCurFbLpfFre; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqFbkLpf.uwKx); ulLpfTm = 1000000 / 100; //1000hz mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqRefforDesat.uwKx); ulLpfTm = 1000000 / 100; //1000hz mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stIqFbkforDesat.uwKx); /* Coefficient update only once */ if (!scm_blCoefUpdateFlg) { /* Deadband compensation coefficient calculate */ dbc_stDbCompCoefIn.uwDeadBandTimeNs = cp_stControlPara.swIPMDeadTimeNs; // unit: ns, Dead band time dbc_stDbCompCoefIn.uwPosSwOnTimeNs = cp_stControlPara.swIPMTurnOnNs; // unit: ns, IPM switch-on time at positive current dbc_stDbCompCoefIn.uwPosSwOffTimeNs = cp_stControlPara.swIPMTurnOnNs; // unit: ns, IPM switch-off time at positive current dbc_stDbCompCoefIn.uwNegSwOnTimeNs = cp_stControlPara.swIPMTurnOnNs; // unit: ns, IPM switch-on time at negative current dbc_stDbCompCoefIn.uwNegSwOffTimeNs = cp_stControlPara.swIPMTurnOnNs; // unit: ns, IPM switch-off time at negative current dbc_stDbCompCoefIn.ulPWMPerUs = PWM_PERIOD_US; // unit: 0.1us, PWM period dbc_stDbCompCoefIn.uwKcoefVtPerAp = cp_stControlPara.swDbcK; // Q6, Deadband compensation slope coefficient dbc_stDbCompCoefIn.uwVBaseVt = VBASE; // Q0, Vbase dbc_stDbCompCoefIn.uwIBaseAp = IBASE; // Q0, Ibase dbc_voDBCompCoef(&dbc_stDbCompCoefIn, &dbc_stDbCompCoef); /* Flux weakening coefficient calculate */ #if(FLUX_MODE == 0) //spdflx_stCtrlCoefIn.swRSpeedPu = (((SLONG)cp_stMotorPara.swRSpeedRpm * (SLONG)cp_stMotorPara.swMotrPolePairs / 60)/ FBASE)*2^15; 化简为如下，避免溢出 spdflx_stCtrlCoefIn.swRSpeedPu = (SWORD)((8192L)*(SLONG)cp_stMotorPara.swRSpeedRpm * (SLONG)cp_stMotorPara.swMotrPolePairs / (15 * FBASE)); spdflx_stCtrlCoefIn.swIdMinPu = (SWORD)((16384L)*(SLONG)cp_stMotorPara.swIdMinA / IBASE); spdflx_stCtrlCoefIn.swIpeakMaxPu = (SWORD)((16384L)*(SLONG)cp_stMotorPara.swIpeakMaxA / IBASE); spdflx_voCoef(&spdflx_stCtrlCoefIn, &spdflx_stCtrlCoef); #elif(FLUX_MODE == 1) flx_stCtrlCoefIn.swIdMaxAp = (SWORD)cp_stMotorPara.swIdMaxA; // Q0,unit: 0.01A flx_stCtrlCoefIn.swIdMinAp = (SWORD)cp_stMotorPara.swIdMinA; // Q0,unit: 0.01A flx_stCtrlCoefIn.uwRsOhm = cp_stMotorPara.swRsOhm; // Q0,unit: 0.1mOhm flx_stCtrlCoefIn.swIdPIOutMinAp = (SWORD)cp_stControlPara.swFwIdPIOutMin; // Q0,unit: 0.01A flx_stCtrlCoefIn.uwCharCurCrossFreqHz = cp_stControlPara.swFwCharCurCrossFre; // Q0,unit: SQRT(1/2piR) flx_stCtrlCoefIn.uwCharCurDampRatio = cp_stControlPara.swFwCharCurDampRatio; // Q0,unit: SQRT(pi/2R) flx_stCtrlCoefIn.uwIdRegKpPu = cp_stControlPara.swFwIdKpPu; // Q16,unit: A/V2 flx_stCtrlCoefIn.uwIdRegKiPu = cp_stControlPara.swFwIdKiPu; // Q16,unit: A/V2 flx_stCtrlCoefIn.uwPWMDutyMax = cp_stControlPara.swFwPWMMaxDuty; // Q0,% flx_stCtrlCoefIn.uwVdcLpfFreqHz = cp_stControlPara.swFwVdcLPFFre; // Q0,unit: Hz flx_stCtrlCoefIn.uwVdcMinCalcTmMs = cp_stControlPara.swFwVdcMinCalTMms; // Q0,unit: ms flx_stCtrlCoefIn.uwFwCurLimAp = cp_stMotorPara.swIpeakMaxA; // Q0,unit: 0.01A flx_stCtrlCoefIn.uwIdMinLimRatio = cp_stControlPara.swFwIdMinLimRatio; // Q0,0.01 flx_stCtrlCoefIn.uwUbVt = VBASE; // Q0,unit: 0.1V, Voltage base flx_stCtrlCoefIn.uwFreqTbcHz = FTBC_HZ; // Q0 flx_stCtrlCoefIn.uwIBaseAp = IBASE; // Q0,unit: 0.01A, Base Current flx_stCtrlCoefIn.uwFBaseHz = FBASE; // Q0,unit: Hz, Base Frequency flx_voCoef(&flx_stCtrlCoefIn, &flx_stCtrlCoef); // fw_stFluxWeakeningCoefInPu // fw_voFluxWeakeningCoef(fw_stFluxWeakeningCoefInPu,flx_stCtrlCoef) #else //Config Error #endif /* Constant vlotage brake coefficient calculate */ cvb_stBrakeCoefIn.uwVdcCvbVt = cp_stControlPara.swCvbConstantVolBrakeV; cvb_stBrakeCoefIn.uwLowSpdRpm = cp_stControlPara.swCvbConstantSpdLowRpm; cvb_stBrakeCoefIn.swIqRefMaxAp = cp_stMotorPara.swIpeakMaxA; cvb_stBrakeCoefIn.swIdRefMaxAp = cp_stMotorPara.swIdMaxA; cvb_stBrakeCoefIn.swIdRefMinAp = cp_stMotorPara.swIdMinA; cvb_stBrakeCoefIn.uwVBaseVt = VBASE; cvb_stBrakeCoefIn.uwIBaseAp = IBASE; cvb_stBrakeCoefIn.uwFBaseHz = FBASE; cvb_stBrakeCoefIn.uwMotorPairs = cp_stMotorPara.swMotrPolePairs; cvb_voBrakeCoef(&cvb_stBrakeCoefIn, &cvb_stBrakeCoef); /* Speed feedback low pass filter coef */ ulLpfTm = 1000000 / cp_stControlPara.swAsrSpdFbLPFFre; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stSpdFbkLpf.uwKx); /* Power limit coef */ ulLpfTm = 1000000 / cp_stControlPara.swPwrLimitLPFFre; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stMotoPwrInLpf.uwKx); ulLpfTm = 1000000 / 10; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stPCBTempLpf.uwKx); ulLpfTm = 1000000 / 10; mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stMotorTempLpf.uwKx); // /* Torque Sensor limit coef */ // ulLpfTm = 1000000 / torsensor_stTorSensorCof.uwTorSensorLPFFrq; // mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stTorSensorLpf.uwKx); // /* Bike Throttle limit coef */ // ulLpfTm = 1000000 / bikethrottle_stBikeThrottleCof.uwThrottleVolLPFFrq; // mth_voLPFilterCoef(ulLpfTm, FTBC_HZ, &scm_stBikeThrottleLpf.uwKx); pwr_stPwrLimCofIn.swPwrLimW = cp_stControlPara.swPwrLimitValWtCALC; // Q0, unit: 0.1w, Power limit value pwr_stPwrLimCofIn.uwPwrErrW = cp_stControlPara.swPwrLimitErrWt; // Q0, unit: 0.1w, Start power limit when "VAL - ERR" pwr_stPwrLimCofIn.swIqMaxAp = cp_stMotorPara.swIpeakMaxA; // Q0, unit: 0.01A, Max phase current (peak value) pwr_stPwrLimCofIn.uwIBaseAp = IBASE; // Q0,unit: 0.01A, Base Current pwr_stPwrLimCofIn.uwUbVt = VBASE; // Q0,unit: 0.1V, Voltage base pwr_stPwrLimCofIn.uwPwrLimPIKp = cp_stControlPara.swPwrLimitKpPu; pwr_stPwrLimCofIn.uwPwrLimPIKi = cp_stControlPara.swPwrLimitKiPu; pwr_stPwrLimCofIn.uwPwrLimSTARTCe = cp_stControlPara.swAlmPwrLimitStartTempVal; pwr_stPwrLimCofIn.uwPwrLimENDCe = cp_stControlPara.swAlmOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimMotTempSTARTCe = cp_stControlPara.swAlmPwrLimitMotorStartTempVal; pwr_stPwrLimCofIn.uwPwrLimMotTempENDCe = cp_stControlPara.swAlmMotorOverHeatCeVal; pwr_stPwrLimCofIn.uwPwrLimStartBatCap = PWRLIM_START_BATCAP; pwr_stPwrLimCofIn.uwPwrLimEndBatCap = PWRLIM_END_BATCAP; pwr_voPwrLimCof(&pwr_stPwrLimCofIn, &pwr_stPwrLimCof); /*Accelaration&Decelaration limit*/ if (abs(scm_swSpdRefPu) < USER_MOTOR_300RPM2PU) { cmd_stCmdCoefIn.ulAccelPu = ulAccel100rpmpsPu; // Q29 } else { cmd_stCmdCoefIn.ulAccelPu = ulAccel100rpmpsPu; // Q29 } cmd_stCmdCoefIn.ulDecelPu = USER_MOTOR_3000RPMPS2PU_Q29; // Q29 cmd_stCmdCoefIn.swBrakeSpdDeltaPu = USER_MOTOR_100RPM2PU; cmd_voCmdCoef(&cmd_stCmdCoefIn, &cmd_stCmdCoef); pwm_stGenCoefIn.uwPWMDutyMax = cp_stControlPara.swPWMMaxDuty; pwm_stGenCoefIn.uwPWM7To5Duty = cp_stControlPara.swPWM7to5Duty; pwm_stGenCoefIn.uwPWMMinSample1Pu = cp_stControlPara.swPWMMinSampleDuty1; pwm_stGenCoefIn.uwPWMMinSample2Pu = cp_stControlPara.swPWMMinSampleDuty2; pwm_stGenCoefIn.uwPWMMinSample3Pu = cp_stControlPara.swPWMMinSampleDuty3; pwm_stGenCoefIn.uwSampleSteadyPu = cp_stControlPara.swPWMSampleToSteady; pwm_stGenCoefIn.uwSingelResisSamplePu = cp_stControlPara.swPWMSampleSigR; pwm_stGenCoefIn.uwOvmNo = cp_stControlPara.swPWMOverMdlMode; pwm_stGenCoefIn.uwPWMPd = HW_PWM_PERIOD; pwm_voGenCoef(&pwm_stGenCoefIn, &pwm_stGenCoef); scm_uwAcrLimCof = (UWORD)((ULONG)cp_stControlPara.swPWMMaxDuty * cp_stControlPara.uwAcrCurOutLim / 1000); // Q15 scm_uwUdcpLimCof = (UWORD)((ULONG)cp_stControlPara.swPWMMaxDuty * cp_stControlPara.uwAcrUdcpOutLim / 1000); // Q15 } } /*************************************************************** Function: scm_voSpdCtrMdTbs; Description: Speed control mode TBS scheduler Call by: tbs_voIsr(); Input Variables: N/A Output/Return Variables: N/A Subroutine Call: ...; Reference: N/A ****************************************************************/ SWORD testIqref; void scm_voSpdCtrMdTbs(void) { SWORD swIqLowerPu; /* Speed feedback LPF */ if (cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER) { mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf); } else if (cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER) { // mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf); } else if (cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL) { scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu; } else {} /* Speed feedback Absolute */ scm_uwSpdFbkLpfAbsPu = abs(scm_stSpdFbkLpf.slY.sw.hi); /*============================================================ Speed command generator to generate speed ramp =============================================================*/ if (curSpeed_state.state == ClzLoop || curSpeed_state.state == Open2Clz) { cmd_stCmdIn.swSpdCmdRpm = uart_slSpdRefRpm; cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi; cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut); scm_swRotateDir = cmd_stCmdOut.swNewCmdDir; scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu; } else if (curSpeed_state.state == StartUp) { SWORD tempSpeed = 0; tempSpeed = (cp_stControlPara.swDragSpdHz * 60 / cp_stMotorPara.swMotrPolePairs); if (cp_stFlg.RunModelSelect == ClZLOOP) { if (uart_slSpdRefRpm > 0) { cmd_stCmdIn.swSpdCmdRpm = tempSpeed; } else { cmd_stCmdIn.swSpdCmdRpm = -tempSpeed; } } else if (cp_stFlg.RunModelSelect == VFContorl || cp_stFlg.RunModelSelect == IFContorl) { if (uart_slSpdRefRpm > 0) { cmd_stCmdIn.swSpdCmdRpm = tempSpeed; } else { cmd_stCmdIn.swSpdCmdRpm = -tempSpeed; } // if(cp_stFlg.RotateDirectionSelect == ForwardRotate) // { // cmd_stCmdIn.swSpdCmdRpm = tempSpeed; // } // else // { // cmd_stCmdIn.swSpdCmdRpm = -tempSpeed; // } } else {} cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi; cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut); scm_swRotateDir = cmd_stCmdOut.swNewCmdDir; scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu; } else { cmd_stCmdIn.swSpdCmdRpm = 0; cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi; cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut); scm_swRotateDir = cmd_stCmdOut.swNewCmdDir; scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; // cmd_stCmdGenOut.Out.swSpdRefPu; } /*======================================================================= Speed PI Controller =======================================================================*/ asr_stSpdPIIn.swSpdRefPu = scm_swSpdRefPu; // Q15 // asr_stSpdPIIn.swSpdFdbPu = switchhall_stOut.swLowSpdPu; asr_stSpdPIIn.swSpdFdbPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15 if (curSpeed_state.state != ClzLoop) { swIqLowerPu = flx_stCtrlOut.swIqLimPu; } else { swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu); swIqLowerPu = (swIqLowerPu < abs(cvb_stBrakeOut.swIqLimPu)) ? swIqLowerPu : abs(cvb_stBrakeOut.swIqLimPu); } if (scm_swRotateDir > 0) { asr_stSpdPIIn.swIqMaxPu = swIqLowerPu; // asr_stSpdPIIn.swIqMinPu = -swIqLowerPu; asr_stSpdPIIn.swIqMinPu = 0; } else { asr_stSpdPIIn.swIqMaxPu = 0; // asr_stSpdPIIn.swIqMaxPu = swIqLowerPu; asr_stSpdPIIn.swIqMinPu = -swIqLowerPu; } asr_voSpdPI(&asr_stSpdPIIn, &asr_stSpdPICoef, &asr_stSpdPIOut); // swCurRefrompu = (abs(asr_stSpdPIOut.swIqRefPu) < abs(uart_swTorqRefNm)) ? abs(asr_stSpdPIOut.swIqRefPu) : abs(uart_swTorqRefNm); // swCurRefrompu = -swCurRefrompu; swCurRefrompu = asr_stSpdPIOut.swIqRefPu; /* New ASR */ // asrnew_stSpdPIIn.swSpdRefPu = scm_swSpdRefPu; // Q15 // asrnew_stSpdPIIn.swSpdFdbPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15 // if (scm_swRotateDir > 0) // { // asrnew_stSpdPIIn.swIqMaxPu = swIqLowerPu; // asrnew_stSpdPIIn.swIqMinPu = 0;//-swIqLowerPu; // } // else // { // asrnew_stSpdPIIn.swIqMaxPu = swIqLowerPu; // asrnew_stSpdPIIn.swIqMinPu = 0;//-swIqLowerPu; // } // asrnew_voSpdPI(&asrnew_stSpdPIIn, &asrnew_stSpdPICoef, &asrnew_stSpdPIOut); // swCurRefrompu = asrnew_stSpdPIOut.swIqRefPu; // swCurRefrompu = testIqref; curSpeed_state.Tbs_hook(); } SWORD deltC, switchCNT, switchflg; SWORD swTmpSpdRate = 0; LPF_OUT swTmpSpdRateLpf; SWORD swTmpSpdFbkPuZ1 = 0; SLONG slTmpAcc; SWORD swTestIqref; void scm_voTorqCtrMdTbs(void) { SWORD swIqLowerPu; /* Speed feedback LPF */ if (cp_stFlg.ThetaGetModelSelect == ANG_OBSERVER) { mth_voLPFilter(obs_stObsOutPu.swElecFreqPu, &scm_stSpdFbkLpf); } else if (cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER) { // mth_voLPFilter(spi_stResolverOut.swSpdFbkPu, &scm_stSpdFbkLpf); } else if (cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL) { scm_stSpdFbkLpf.slY.sw.hi = switchhall_stOut.swLowSpdLpfPu; } else {} /* Speed feedback Absolute */ scm_uwSpdFbkLpfAbsPu = abs(scm_stSpdFbkLpf.slY.sw.hi); // /*============================================================ // Speed command generator to generate speed ramp // =============================================================*/ // cmd_stCmdIn.swSpdCmdRpm = -(((SLONG)cadence_stFreGetOut.uwLPFFrequencyPu * 8000) >> 10) * 6000 >> 15; // cmd_stCmdIn.swSpdNowPu = scm_stSpdFbkLpf.slY.sw.hi; // cmd_voCmdOut(&cmd_stCmdIn, &cmd_stCmdCoef, &cmd_stCmdOut); // /*======================================================================= // Get speed command // =======================================================================*/ // scm_swRotateDir = cmd_stCmdOut.swNewCmdDir; // scm_swSpdRefPu = cmd_stCmdOut.swIntRefPu; //cmd_stCmdGenOut.Out.swSpdRefPu; /*======================================================================= Speed PI Controller =======================================================================*/ swIqLowerPu = (flx_stCtrlOut.swIqLimPu < abs(pwr_stPwrLimOut2.swIqLimPu)) ? flx_stCtrlOut.swIqLimPu : abs(pwr_stPwrLimOut2.swIqLimPu); swIqLowerPu = (swIqLowerPu < abs(cvb_stBrakeOut.swIqLimPu)) ? swIqLowerPu : abs(cvb_stBrakeOut.swIqLimPu); // if (uart_swTorqRefNm > swIqLowerPu) // { // uart_swTorqRefNm = swIqLowerPu; // } // else if (uart_swTorqRefNm < -swIqLowerPu) // { // uart_swTorqRefNm = -swIqLowerPu; // } // else // {} // swCurRefrompu = uart_swTorqRefNm; /* Torque observe */ if (scm_swRotateDir > 0) { torqobs_stCalIn.swIqMaxPu = swIqLowerPu; torqobs_stCalIn.swIqMinPu = -swIqLowerPu; } else { torqobs_stCalIn.swIqMaxPu = swIqLowerPu; torqobs_stCalIn.swIqMinPu = -swIqLowerPu; } torqobs_stCalIn.swIqfbkPu = scm_swIqFdbLpfPu; torqobs_stCalIn.swSpdPu = switchhall_stOut.swLowSpdPu; torqobs_voCal(&torqobs_stCalIn, &torqobs_stCoef, &torqobs_stCalOut); mth_voLPFilter((torqobs_stCalOut.swIqLoadPu + scm_swIqFdbLpfPu), &scm_stIqLoadLpf); // mth_voLPFilter(LoadObsTheta_Y.swIqCompPu, &scm_stIqLoadLpf); ///////test//////// // mth_voLPFilterCoef(1000000 / 15, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS // mth_voLPFilter(LoadObsTheta_Y.swIqCompPu, &swTmpSpdRateLpf); /* Spd Fbk Compensation Calculate */ // swTmpSpdRate = switchhall_stOut.swLowSpdPu - swTmpSpdFbkPuZ1; //Q15 // mth_voLPFilterCoef(1000000 / 30, FTBS_HZ, &swTmpSpdRateLpf.uwKx); //30Hz,TBS // mth_voLPFilter(swTmpSpdRate, &swTmpSpdRateLpf); // swTmpSpdFbkPuZ1 = switchhall_stOut.swLowSpdPu; // slTmpAcc = (SLONG)swTmpSpdRateLpf.slY.sw.hi * FTBS_HZ / FBASE; //Q15 /* Iqref Compensation */ if (((uart_swTorqRefNm < -200) || (uart_swTorqRefNm > 200))) { /* Without Comp */ swTestIqref = uart_swTorqRefNm; /* Open Loop */ // swTestIqref = uart_swTorqRefNm - (((SLONG)slTmpAcc* cof_uwJmPu << 11) / cof_uwFluxPu); //Q15+Q0+Q11-Q12=Q14 /* Observer */ // swTestIqref = uart_swTorqRefNm - scm_stIqLoadLpf.slY.sw.hi; // swTestIqref = uart_swTorqRefNm - swTmpSpdRateLpf.slY.sw.hi; } else { swTestIqref = uart_swTorqRefNm; } if (swTestIqref > swIqLowerPu) { swTestIqref = swIqLowerPu; } else if (swTestIqref < -swIqLowerPu) { swTestIqref = -swIqLowerPu; } else {} swCurRefrompu = swTestIqref; curSpeed_state.Tbs_hook(); } /*************************************************************** Function: scm_voSpdCtrMdUpTbc; Description: Speed control mode TBC scheduler Call by: tbc_voIsr(); Input Variables: N/A Output/Return Variables: N/A Subroutine Call: ...; Reference: N/A ****************************************************************/ CRD_PARK_IN Test_U_in; CRD_PARK_OUT Test_U_out; void scm_voSpdCtrMdUpTbc(void) { /*======================================================================= Max voltage of current PI out =======================================================================*/ scm_swVsLimPu = (SWORD)((ULONG)adc_stUpOut.uwVdcLpfPu * scm_uwAcrLimCof >> 15); // Q14+Q15-Q15=Q14 scm_swVsDcpLimPu_Assist = (SWORD)((ULONG)adc_stUpOut.uwVdcLpfPu * scm_uwUdcpLimCof >> 15); // Q14+Q15-Q15=Q14 scm_swVsDcpLimPu = scm_swVsDcpLimPu_Assist; /*======================================================================= Voltage get =======================================================================*/ /* Get Ualpha & Ubeta from command voltage */ scm_swUalphaPu = pwm_stGenOut.swUalphaPu - scm_swUalphaCompPu; // Q14 scm_swUbetaPu = pwm_stGenOut.swUbetaPu - scm_swUbetaCompPu; // Q14 /*======================================================================= Startup control FSM =======================================================================*/ scm_voSpdCtrMdFSM(); curSpeed_state.Tbcup_hook(); } /*************************************************************** Function: scm_voSpdCtrMdTbc; Description: Speed control mode TBC scheduler Call by: tbc_voIsr(); Input Variables: N/A Output/Return Variables: N/A Subroutine Call: ...; Reference: N/A ****************************************************************/ UWORD DCPswitch = 0; UWORD testtheta = 0; SWORD IQqqqqq = 0, SwitchFlggg = 1; SLONG IQCNT = 0; UWORD uwIqStopCnt; SLONG swUqmax,swUqmin; SLONG swUdmax,swUdmin; UWORD UqDecCount = 0, UdDecCount = 0; void scm_voSpdCtrMdDownTbc(void) { /*======================================================================= Clark transformation for phase current =======================================================================*/ crd_stClarkIn.swAPu = adc_stDownOut.swIaPu; // Q14 crd_stClarkIn.swBPu = adc_stDownOut.swIbPu; // Q14 crd_stClarkIn.swCPu = adc_stDownOut.swIcPu; // Q14 crd_voClark(&crd_stClarkIn, &crd_stCurClarkOut); /*======================================================================= Code Of spdFSM =======================================================================*/ curSpeed_state.Tbcdown_hook(); // if(SwitchFlggg == 0) // { // IQCNT++; // scm_swIqRefPu = -4000; // if(IQCNT > 2000) // { // IQCNT=0; // SwitchFlggg =1; // } // } // else // { // scm_swIqRefPu = IQqqqqq; // } // /*======================================================================= Current loop control =======================================================================*/ /* Get Id & Iq for current PI control */ /*======================================================================= Park transformation for current =======================================================================*/ crd_stParkIn.swAlphaPu = crd_stCurClarkOut.swAlphaPu; // Q14 crd_stParkIn.swBetaPu = crd_stCurClarkOut.swBetaPu; // Q14 crd_stParkIn.uwThetaPu = scm_uwAngParkPu; // Q15 crd_voPark(&crd_stParkIn, &crd_stCurParkOut); /*======================================================================= Current feedback LPF =======================================================================*/ mth_voLPFilter(crd_stCurParkOut.swDPu, &scm_stIdFbkLpf); mth_voLPFilter(crd_stCurParkOut.swQPu, &scm_stIqFbkLpf); scm_swIdFdbLpfPu = scm_stIdFbkLpf.slY.sw.hi; scm_swIqFdbLpfPu = scm_stIqFbkLpf.slY.sw.hi; // scm_swIdFdbLpfPu = crd_stCurParkOut.swDPu; // scm_swIqFdbLpfPu = crd_stCurParkOut.swQPu; /*======================================================================= Calculate input power of motor =======================================================================*/ scm_swSystemPwrInPu = ((((SLONG)adc_stUpOut.uwVdcLpfPu * (SLONG)adc_stUpOut.uwIbusAvgLpfPu) >> 13) * (SLONG)683) >> 10; // power = udc*idc; Q14+Q14-Q13=Q15 scm_swMotorPwrInPu = ((SLONG)Test_U_out.swDPu * scm_swIdFdbLpfPu + (SLONG)Test_U_out.swQPu * scm_swIqFdbLpfPu) >> 13; // Q14+Q14-Q13=Q15 mth_voLPFilter(scm_swMotorPwrInPu, &scm_stMotoPwrInLpf); scm_swMotorPwrInLpfWt = scm_stMotoPwrInLpf.slY.sw.hi * cof_uwPbWt >> 15; // unit: 0.1w /*======================================================================= Id current PI control =======================================================================*/ DCPswitch = 0; // 0 with forwardFeedBack 1 without forwardFeedBack acr_stCurIdPIIn.swCurRefPu = scm_swIdRefPu; // Q14 acr_stCurIdPIIn.swCurFdbPu = scm_swIdFdbLpfPu; if (DCPswitch == 1) { acr_stCurIdPIIn.swUmaxPu = scm_swVsDcpLimPu; // Q14 acr_stCurIdPIIn.swUminPu = -scm_swVsDcpLimPu; // Q14 } else if (DCPswitch == 0) { if(switch_flg.SysRun_Flag == FALSE) { if(UdDecCount>=100) // each 100/8000s decrease to 1000/1024*U { UdDecCount = 0; acr_stCurIdPIIn.swUmaxPu = (SWORD)(((SLONG)swUdmax*1020)>>10); acr_stCurIdPIIn.swUminPu = (SWORD)(((SLONG)swUdmin*1020)>>10); } else { UdDecCount++; acr_stCurIdPIIn.swUmaxPu = swUdmax; // Q14 acr_stCurIdPIIn.swUminPu = swUdmin; // Q14 } } else { acr_stCurIdPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu; // Q14 acr_stCurIdPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUdPu; // Q14 } } else {} swUdmin = acr_stCurIqPIIn.swUminPu; swUdmax = acr_stCurIqPIIn.swUmaxPu; acr_voCurPI(&acr_stCurIdPIIn, &acr_stCurIdPICoef, &acr_stCurIdPIOut); /*======================================================================= Iq current PI control =======================================================================*/ mth_voLPFilter(scm_swIqRefPu, &scm_stIqRefforDesat); mth_voLPFilter(scm_swIqFdbLpfPu, &scm_stIqFbkforDesat); //-------------limit min vault if (cp_stFlg.RotateDirectionSelect == ForwardRotate) { if(acr_stUdqDcpOut.swUqPu<=0) acr_stUdqDcpOut.swUqPu=0; } else if (cp_stFlg.RotateDirectionSelect == BackwardRotate) { if(acr_stUdqDcpOut.swUqPu>=0) acr_stUdqDcpOut.swUqPu=0; } //------------------ if (DCPswitch == 1) { acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu; // Q14 acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu; // Q14 } else if (DCPswitch == 0) { if(event_blCurrentAssFlag == TRUE) { if (cp_stFlg.RotateDirectionSelect == ForwardRotate) { if (((SLONG)ass_stCadAssCalOut.swVoltLimitPu - (SLONG)acr_stUdqDcpOut.swUqPu) > 0) { acr_stCurIqPIIn.swUmaxPu = ass_stCadAssCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14 } else { acr_stCurIqPIIn.swUmaxPu = 0; } if(ass_stCadAssCalOut.blPreStopFlag == TRUE) { acr_stCurIqPIIn.swUminPu = 0; } else { acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; } } else if (cp_stFlg.RotateDirectionSelect == BackwardRotate) { if (((SLONG)ass_stCadAssCalOut.swVoltLimitPu - (SLONG)acr_stUdqDcpOut.swUqPu) < 0) { acr_stCurIqPIIn.swUminPu = ass_stCadAssCalOut.swVoltLimitPu - acr_stUdqDcpOut.swUqPu; // Q14 } else { acr_stCurIqPIIn.swUminPu = 0; } if(ass_stCadAssCalOut.blPreStopFlag == TRUE ) { acr_stCurIqPIIn.swUmaxPu = 0; } else { acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; } } else { /* 方向错误 */ } } else { if(switch_flg.SysRun_Flag == FALSE) { if(UqDecCount>=100) // each 100/8000s decrease to 1000/1024*U { UqDecCount = 0; acr_stCurIqPIIn.swUmaxPu = (SWORD)(((SLONG)swUqmax*1010)>>10); acr_stCurIqPIIn.swUminPu = (SWORD)(((SLONG)swUqmin*1010)>>10); } else { UqDecCount++; acr_stCurIqPIIn.swUmaxPu = swUqmax; // Q14 acr_stCurIqPIIn.swUminPu = swUqmin; // Q14 } } else { acr_stCurIqPIIn.swUmaxPu = scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; // Q14 acr_stCurIqPIIn.swUminPu = -scm_swVsDcpLimPu - acr_stUdqDcpOut.swUqPu; // Q14 } } } else {} swUqmin = acr_stCurIqPIIn.swUminPu; swUqmax = acr_stCurIqPIIn.swUmaxPu; #if 0 if(0 == scm_swIqRefPu) { uwIqStopCnt++; if(uwIqStopCnt >= 500) { uwIqStopCnt = 500; } } else { uwIqStopCnt = 0; } if((500 == uwIqStopCnt) && (scm_uwSpdFbkLpfAbsPu < 1500)) { if((cp_stFlg.RotateDirectionSelect == BackwardRotate) && (scm_swIqFdbLpfPu > 0)) { acr_stCurIqPIIn.swCurRefPu = 0; // Q14 acr_stCurIqPIIn.swCurFdbPu = 0; acr_stCurIqPIOut.swURefPu=0; acr_stCurIqPIOut.slURefPu=0; scm_swUqRefPu=0; } else if((cp_stFlg.RotateDirectionSelect == ForwardRotate) && (scm_swIqFdbLpfPu < 0)) { acr_stCurIqPIIn.swCurRefPu = 0; // Q14 acr_stCurIqPIIn.swCurFdbPu = 0; acr_stCurIqPIOut.swURefPu=0; acr_stCurIqPIOut.slURefPu=0; scm_swUqRefPu=0; } else { acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14 acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu; } } else { acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14 acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu; } #else acr_stCurIqPIIn.swCurRefPu = scm_swIqRefPu; // Q14 acr_stCurIqPIIn.swCurFdbPu = scm_swIqFdbLpfPu; #endif acr_voCurPI(&acr_stCurIqPIIn, &acr_stCurIqPICoef, &acr_stCurIqPIOut); // if ((DCPswitch == 1) && (scm_uwSpdFbkLpfAbsPu > 30922)) // Q15 2000rpm // { // acr_stCurIdPIOut.slURefPu = acr_stCurIdPIOut.slURefPu - ((SLONG)acr_stUdqDcpOut.swUdPu << 15); // acr_stCurIqPIOut.slURefPu = acr_stCurIqPIOut.slURefPu - ((SLONG)acr_stUdqDcpOut.swUqPu << 15); // acr_stCurIdPIOut.swURefPu = acr_stCurIdPIOut.swURefPu - acr_stUdqDcpOut.swUdPu; // acr_stCurIqPIOut.swURefPu = acr_stCurIqPIOut.swURefPu - acr_stUdqDcpOut.swUqPu; // DCPswitch = 0; // } // if ((DCPswitch == 0) && (scm_uwSpdFbkLpfAbsPu < 8192)) // { // acr_stCurIdPIOut.slURefPu = acr_stCurIdPIOut.slURefPu + ((SLONG)acr_stUdqDcpOut.swUdPu << 15); // acr_stCurIqPIOut.slURefPu = acr_stCurIqPIOut.slURefPu + ((SLONG)acr_stUdqDcpOut.swUqPu << 15); // acr_stCurIdPIOut.swURefPu = acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu; // acr_stCurIqPIOut.swURefPu = acr_stCurIqPIOut.swURefPu + acr_stUdqDcpOut.swUqPu; // DCPswitch = 1; // } if (DCPswitch == 1) { scm_swUqRefPu = acr_stCurIqPIOut.swURefPu; // Q14 scm_swUdRefPu = acr_stCurIdPIOut.swURefPu; // Q14 } else if (DCPswitch == 0) { scm_swUqRefPu = acr_stCurIqPIOut.swURefPu + acr_stUdqDcpOut.swUqPu; // Q14 scm_swUdRefPu = acr_stCurIdPIOut.swURefPu + acr_stUdqDcpOut.swUdPu; // Q14 } else {} if(event_blCurrentAssFlag == TRUE) { if (cp_stFlg.RotateDirectionSelect == ForwardRotate) { if (scm_swUqRefPu > ass_stCadAssCalOut.swVoltLimitPu) { scm_swUqRefPu = ass_stCadAssCalOut.swVoltLimitPu; // Q14=Q14 } } else if (cp_stFlg.RotateDirectionSelect == BackwardRotate) { if (scm_swUqRefPu < ass_stCadAssCalOut.swVoltLimitPu) { scm_swUqRefPu = ass_stCadAssCalOut.swVoltLimitPu; // Q14=Q14 } } else { /* 方向错误 */ } } else {} /*======================================================================= IPark transformation for current =======================================================================*/ #if(EMCDEAL_EN!=0) if(EcmDeal.EmcModeFlag==FALSE) { EcmDeal.swUdRefPu=scm_swUdRefPu; EcmDeal.swUqRefPu=scm_swUqRefPu; } else { scm_swUdRefPu=EcmDeal.swUdRefPu; scm_swUqRefPu=EcmDeal.swUqRefPu; } #endif //-----------------2024 // scm_swUdRefPu=0; // if (cp_stFlg.RotateDirectionSelect == ForwardRotate) // { // if(bikethrottle_stBikeThrottleOut.uwThrottlePercent>250) // { // scm_swUqRefPu=bikethrottle_stBikeThrottleOut.uwThrottlePercent*10-2000; // } // else // { // if(scm_swUqRefPu>0) // scm_swUqRefPu--; // else if(scm_swUqRefPu<0) // scm_swUqRefPu=0; // // // } // // } // else if (cp_stFlg.RotateDirectionSelect == BackwardRotate) // { // if(bikethrottle_stBikeThrottleOut.uwThrottlePercent>250) // { // scm_swUqRefPu=-bikethrottle_stBikeThrottleOut.uwThrottlePercent*10+2000; // } // else // { // if(scm_swUqRefPu<0) // scm_swUqRefPu++; // else if(scm_swUqRefPu>0) // scm_swUqRefPu=0; // // // } // } //---------------------- crd_stIParkIn.swDPu = scm_swUdRefPu; crd_stIParkIn.swQPu = scm_swUqRefPu; crd_stIParkIn.uwThetaPu = scm_uwAngIParkPu; crd_voIPark(&crd_stIParkIn, &crd_stVltIParkOut); /*======================================================================= Deadband compensation =======================================================================*/ #if (0) dbc_stDbCompIn.swIaPu = adc_stDownOut.swIaPu; // Q14 dbc_stDbCompIn.swIbPu = adc_stDownOut.swIbPu; // Q14 dbc_stDbCompIn.swIcPu = adc_stDownOut.swIcPu; // Q14 dbc_stDbCompIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu; // Q14 dbc_stDbCompIn.swWsPu = scm_stSpdFbkLpf.slY.sw.hi; // Q15 // dbc_stDbCompCoef.uwNegWinVoltDuty = mn_uwNegWinVoltDuty; // dbc_stDbCompCoef.uwPosLostVoltDuty = mn_uwPosLostVoltDuty; dbc_voDBComp(&dbc_stDbCompIn, &dbc_stDbCompCoef, &dbc_stDbCompOut); #endif scm_swUalphaRefPu = crd_stVltIParkOut.swAlphaPu + dbc_stDbCompOut.swUalphaCompPu; // Q14 scm_swUbetaRefPu = crd_stVltIParkOut.swBetaPu + dbc_stDbCompOut.swUbetaCompPu; // Q14 scm_swUalphaCompPu = dbc_stDbCompOut.swUalphaCompPu; // Q14 scm_swUbetaCompPu = dbc_stDbCompOut.swUbetaCompPu; // Q14 /*======================================================================= PWM generate =======================================================================*/ if (cp_stFlg.RunModelSelect == VFContorl) { SWORD swVFVolAmp = 0; swVFVolAmp = ((SLONG)cp_stControlPara.swDragVolAp << 14) / VBASE; if (cp_stFlg.RotateDirectionSelect == ForwardRotate) { crd_stIParkIn.swDPu = 0; crd_stIParkIn.swQPu = swVFVolAmp; } else if (cp_stFlg.RotateDirectionSelect == BackwardRotate) { crd_stIParkIn.swDPu = 0; crd_stIParkIn.swQPu = swVFVolAmp; } else {} crd_stIParkIn.uwThetaPu = scm_uwAngIParkPu; // scm_uwAngIParkPu; crd_voIPark(&crd_stIParkIn, &crd_stVltIParkOut); scm_swUalphaRefPu = crd_stVltIParkOut.swAlphaPu; scm_swUbetaRefPu = crd_stVltIParkOut.swBetaPu; } pwm_stGenIn.swUalphaPu = scm_swUalphaRefPu; // Q14 pwm_stGenIn.swUbetaPu = scm_swUbetaRefPu; // Q14 pwm_stGenIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu; // Q14 pwm_voGen(&pwm_stGenIn, &pwm_stGenCoef, &pwm_stGenOut); // iPwm_SetCompareGroupValues16(0, pwm_stGenOut.uwNewTIM1COMPR); // if (cp_stFlg.CurrentSampleModelSelect == SINGLERESISITANCE) // { // ULONG samplingTick[2]; // samplingTick[0] = pwm_stGenOut.uwFirstTrigCOMPR; // samplingTick[1] = pwm_stGenOut.uwSecondTrigCOMPR; // //iPwm_SyncMultiSamplingCountUp(0, &samplingTick[0], 2); // } Test_U_in.swAlphaPu = pwm_stGenOut.swUalphaPu - scm_swUalphaCompPu; // Q14 Test_U_in.swBetaPu = pwm_stGenOut.swUbetaPu - scm_swUbetaCompPu; // Q14 Test_U_in.uwThetaPu = scm_uwAngIParkPu; // Q15 crd_voPark(&Test_U_in, &Test_U_out); //// pwm_stGenOut.uwNewTIM1COMPR[0] = 500; //// pwm_stGenOut.uwNewTIM1COMPR[1] = 500; //// pwm_stGenOut.uwNewTIM1COMPR[2] = 500; //// pwm_stGenOut.uwNewTIM1COMPR[3] = 1700; //// pwm_stGenOut.uwNewTIM1COMPR[4] = 1700; //// pwm_stGenOut.uwNewTIM1COMPR[5] = 1700; } /************************************************************************* Local Functions (N/A) *************************************************************************/ /************************************************************************* Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd. All rights reserved. *************************************************************************/ #ifdef _SPDCTRMODE_C_ #undef _SPDCTRMODE_C_ #endif /************************************************************************* End of this File (EOF)! Do not put anything after this part! *************************************************************************/