1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192 |
- /************************************************************************
- 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!
- *************************************************************************/
|