motor_ctl-1/1.FrameLayer/Source/tbc.c

/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            tbc.c
 Partner Filename:    tbc.h
 Description:         Time base for current loop
 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 _TBC_C_
#define _TBC_C_
#endif

/************************************************************************
 Included File
*************************************************************************/
#include "LoadObsTheta.h"
#include "syspar.h"
#include "user.h"
#include "FSM_1st.h"
#include "FSM_2nd.h"
#include "spdctrmode.h"
#include "queue.h"
#include "canAppl.h"
#include "alarm.h"
#include "AngleObserver_discrete.h"
#include "tmag5273.h"
//#include "profiler.h"
//#include "STLmain.h"
extern ExtU_LoadObsTheta_T LoadObsTheta_U;

/*************************************************************************
 Exported Functions (N/A)
*************************************************************************/
/***************************************************************
 Function: tbc_voIsr;
 Description: TBC interrupt service
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
UWORD tst_CadIO = 0;
void tbc_voUpIsr(void)
{
//    PROFILER_START(1);
//    uart_voAppMonitor();
//    switchhall_voPosCalTbc();
//    tst_CadIO = iGpio_Read(HW_GPIO_CADENCE_PIN);
////     /* Full Order Observer */
////     LoadObsTheta_U.IqFbkPu = scm_swIqFdbLpfPu;
////     LoadObsTheta_U.ThetamPu = switchhall_stOut.uwLowThetaPu;
////     LoadObsTheta_step();
//
//    /* Full Order Observer: Hall Angle Filter */
//    rtU.AngleIn = switchhall_stOut.uwLowThetaPu;
//    Trigger();
//
//    FSM1st_Sys_state.Tbcup_hook();
//    PROFILER_END(1);
    
//    if( cp_stFlg.ThetaGetModelSelect == ANG_RESOLVER )
//     {
//         /*Resolver lock data*/
//         //spi_voResolverLock();
//         /* Resolver data read */
//        // spi_voResolver(&spi_stResolverCoef, &spi_stResolverOut);
//     }
//     else if( cp_stFlg.ThetaGetModelSelect == ANG_SWITCHHALL )
//     {
//         /*Switchhall Position Cal*/
//
//     }
//     else
//     {
//
//     }


//    switchhall_voPosCalTbc(); //角度获取
    /* Full Order Observer */
//      LoadObsTheta_U.IqFbkPu = scm_swIqFdbLpfPu;
//      LoadObsTheta_U.ThetamPu = switchhall_stOut.uwLowThetaPu;
//      LoadObsTheta_step();
//    if(cp_ulSystickCnt > 3000)
//    {
//        Tmag5273_out.Angle = tmag5273_GetAngle();
//    }
    /* Full Order Observer: Hall Angle Filter */
    rtU.AngleIn = switchhall_stOut.uwLowThetaPu;
    Trigger();

    /* Alarm detect */
    alm_voDetecTBS(&alm_stIn, &alm_stDetectTbsCoef);

    FSM1st_Sys_state.Tbcup_hook(); //初始状态采集零点AD值，有故障时判断

}

/***************************************************************
 Function: tbc_voIsr;
 Description: TBC interrupt service
 Call by:
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: ...;
 Reference: N/A
****************************************************************/
void tbc_voDownIsr(void)
{
   // tst_CadIO = iGpio_Read(HW_GPIO_CADENCE_PIN);
    adc_voSampleUp(&adc_stCof, &adc_stUpOut);
//    if((FALSE == UART_bInsertPendTx)&&(UART_bMonSwitch == FALSE))
//    {
//      stl_voTbcProc();
//    }
    /* Alarm detect */
    alm_stIn.blADCInitOvrFlg = sysfsm_stFlg.blADCInitOvrFlg;

    alm_stIn.uwIpeakPu = adc_stDownOut.uwIpeakPu;    // Q14
    alm_stIn.swMotorPwrInWt = scm_swMotorPwrInLpfWt; // unit: 0.1w
    alm_stIn.uwIaAbsPu = adc_stDownOut.uwIaAbsPu;
    alm_stIn.uwIbAbsPu = adc_stDownOut.uwIbAbsPu;      // Q14
    alm_stIn.uwIcAbsPu = adc_stDownOut.uwIcAbsPu;      // Q14
    alm_stIn.swIalhpaPu = crd_stCurClarkOut.swAlphaPu; // Q14
    alm_stIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
    alm_stIn.swIqRefPu = scm_swIqRefPu;
    alm_stIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;        // Q14
    alm_stIn.uwVdcCompPu = adc_stUpOut.uwVdcLpfPu + BMS_VoltEstimat.uwVdcCompPu; // Q14
    alm_stIn.uwSpdFbkLpfAbsPu = scm_uwSpdFbkLpfAbsPu; // Q15
    if (scm_swSpdRefPu >= 0)
    {
        alm_stIn.uwSpdRefAbsPu = (UWORD)scm_swSpdRefPu;
    }
    else
    {
        alm_stIn.uwSpdRefAbsPu = (UWORD)(-scm_swSpdRefPu);
    }
    alm_stIn.swIPMTempCe = adc_stUpOut.PCBTemp;
    alm_stIn.uwMotorTempCe = adc_stUpOut.MotorTemp;
    alm_stIn.uwIdcOffset = adc_stCof.uwIdcOffset;
    alm_stIn.uwSectorNum = switchhall_stOut.uwSectorNum;
    alm_voDetecTBC(&alm_stIn, &alm_stDetectTbcCoef);


    /* System FSM */
    if (adc_stDownOut.blADCCalibFlg!=0)//自检完再切换状态机
    {
        switch_flg.SysFault_Flag = alm_blAlmOccrFlg;
        switch_flg.SysWarnning_Flag = alm_blWarnOccrFlg;
    }
    
//    if (switch_flg.SysFault_Flag == TRUE)
//    {
//        que_stErrorLog.ErrorCode = alm_unCode.all;
//        que_stErrorLog.RunTime = MC_RunInfo.Ride_Time;
//        que_stErrorLog.RunInfo = MC_RunInfo;
//        que_stErrorLog.IqCurrentPu = scm_swIqFdbLpfPu;
//        que_stErrorLog.IqVoltagePu = scm_swUqRefPu;
//        que_stErrorLog.IdCurrentPu = scm_swIdFdbLpfPu;
//        que_stErrorLog.IdVoltagePu = scm_swUdRefPu;
//
//        //      que_ubPushIn( &stFlashErrorLog, &stErrorLog, 54 );
//    }

    if (adc_stDownOut.blADCCalibFlg && adc_stUpOut.blADCCalibFlg)
    {
        sysfsm_stFlg.blADCInitOvrFlg = TRUE;
    }
    else
    {
        sysfsm_stFlg.blADCInitOvrFlg = FALSE;
    }


    /* ADC sample */
    adc_voSampleDown(&adc_stCof, &adc_stDownOut);


    /* System FSM */
    FSM1st_Sys_state.TbcDown_hook();

   
//  adc_voSampleUp(&adc_stCof, &adc_stUpOut); //电流获取
//      /* Alarm detect */
//      alm_stIn.blADCInitOvrFlg = sysfsm_stFlg.blADCInitOvrFlg;
//
//      alm_stIn.uwIpeakPu = adc_stDownOut.uwIpeakPu;    // Q14
//      alm_stIn.swMotorPwrInWt = scm_swMotorPwrInLpfWt; // unit: 0.1w
//      alm_stIn.uwIaAbsPu = adc_stDownOut.uwIaAbsPu;
//
//      alm_stIn.uwIbAbsPu = adc_stDownOut.uwIbAbsPu;      // Q14
//      alm_stIn.uwIcAbsPu = adc_stDownOut.uwIcAbsPu;      // Q14
//      alm_stIn.swIalhpaPu = crd_stCurClarkOut.swAlphaPu; // Q14
//      alm_stIn.swIbetaPu = crd_stCurClarkOut.swBetaPu;   // Q14
//      alm_stIn.swIqRefPu = scm_swIqRefPu;
//      alm_stIn.uwVdcPu = adc_stUpOut.uwVdcLpfPu;        // Q14
//      alm_stIn.uwSpdFbkLpfAbsPu = scm_uwSpdFbkLpfAbsPu; // Q15
//      if (scm_swSpdRefPu >= 0)
//      {
//          alm_stIn.uwSpdRefAbsPu = (UWORD)scm_swSpdRefPu;
//      }
//      else
//      {
//          alm_stIn.uwSpdRefAbsPu = (UWORD)(-scm_swSpdRefPu);
//      }
//      alm_stIn.uwIPMTempCe = adc_stUpOut.PCBTemp;
//      alm_stIn.uwIdcOffset = adc_stCof.uwIdcOffset;
//      alm_stIn.swDrumSpdAbsNowRpm = uart_swDrumSpdAbsNowRpm;
//      alm_stIn.swDrumSpdAbsPreRpm = uart_swDrumSpdAbsPreRpm;
//      alm_stIn.uwSectorNum = switchhall_stOut.uwSectorNum;
//      alm_stIn.uwBikeFrequencyPu = bikespeed_stFreGetOut.uwLPFFrequencyPu;
//      alm_stIn.uwCadFrequencyPu = cadence_stFreGetOut.uwFreqPercent;
//     // alm_voDetecTBC(&alm_stIn, &alm_stDetectTbcCoef); //缺相过流堵转故障判断
//      sys_unFaultCode.unalmcode = alm_unCode;
//
//      /* System FSM */
//      switch_flg.SysFault_Flag = alm_blAlmOccrFlg;
//      if (switch_flg.SysFault_Flag == TRUE)
//      {
//          stErrorLog.ErrorCode = alm_unCode.all;
//          stErrorLog.RunTime = MC_RunInfo.Ride_Time;
//          stErrorLog.RunInfo = MC_RunInfo;
//          stErrorLog.IqCurrentPu = scm_swIqFdbLpfPu;
//          stErrorLog.IqVoltagePu = scm_swUqRefPu;
//          stErrorLog.IdCurrentPu = scm_swIdFdbLpfPu;
//          stErrorLog.IdVoltagePu = scm_swUdRefPu;
//
//          //      que_ubPushIn( &stFlashErrorLog, &stErrorLog, 54 );
//      }
//
//      if (adc_stDownOut.blADCCalibFlg && adc_stUpOut.blADCCalibFlg)
//      {
//          sysfsm_stFlg.blADCInitOvrFlg = TRUE;
//      }
//      else
//      {
//          sysfsm_stFlg.blADCInitOvrFlg = FALSE;
//      }
//
//
//      /* ADC sample */
//      adc_voSampleDown(&adc_stCof, &adc_stDownOut); //相电流采样读取
//
//      /* ADC Rdson calibration with single resistance*/ //单电阻采样
//      adc_voSRCalibration(&adc_stCof, &adc_stUpOut, &adc_stDownOut);
//
//      /* System FSM */
//      FSM1st_Sys_state.TbcDown_hook();

}

/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

/*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************/
#ifdef _TBC_C_
#undef _TBC_C_
#endif
/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/