motor_ctl-4/User project/3.BasicFunction/Source/spi_master.c

/************************************************************************
 Project:               Welling Motor Control Paltform
 Filename:              spi_master.c
 Partner Filename:      spi_master.h
 Description:           SPI master driver
 Complier:              IAR Embedded Workbench for ARM 8.40.2
 CPU TYPE :             GD32F30x
*************************************************************************
 Copyright (c) 2022 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 _SPI_MASTER_C_
#define _SPI_MASTER_C_
#endif

/************************************************************************
 Included File
*************************************************************************/
#include "syspar.h"
#include "user.h"

/*************************************************************************
 Exported Functions (N/A)
*************************************************************************/
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
void spi_voResolverInit(void)
{
    spi_stResolverOut.uwSpiThetaTmpZ1Pu = 0;
    spi_stResolverOut.swSpdLpfTmpPu = 0;
    spi_stResolverOut.swSpdLpfTmpZ1Pu = 0;
    spi_stResolverOut.uwSpiThetaPu = 0;
    spi_stResolverOut.uwSpiOrignData = 0;
    spi_stResolverOut.slPllThetaPu = 0;
    spi_stResolverOut.uwPllThetaPu = 0;
    spi_stResolverOut.slThetaErrPu = 0;
    spi_stResolverOut.slThetaErrZ1Pu = 0;
    spi_stResolverOut.slThetaDeltaErrPu = 0;
    spi_stResolverOut.swSpdFbkPu = 0;
    spi_stResolverOut.swPllSpdFbkPu = 0;
    spi_stResolverOut.slPllSpdFbkPu = 0;
    spi_stResolverOut.swSpdFbkLpfPu = 0;
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
void spi_voResolverCoef(SPI_RESOLVER_COEFIN *in, SPI_RESOLVER_COEF *out)
{
    UWORD uwMvcPu;
    ULONG ulDamper;
    UWORD uwDamper;

    if (in->uwFbHz < 10)
    {
        in->uwFbHz = 10;
    }
    else if (in->uwFbHz > 10000)
    {
        in->uwFbHz = 10000;
    }

    if (in->uwFreqTbcHz < 10)
    {
        in->uwFreqTbcHz = 10;
    }

    if (in->uwSpdPllMcoef > 100)
    {
        in->uwSpdPllMcoef = 100;
    }

    out->uwCurTs = ((ULONG)in->uwFbHz << 10) / in->uwFreqTbcHz;      // Q10, TBC time
    out->uwCurTsPu = ((ULONG)205887 * in->uwFbHz) / in->uwFreqTbcHz; // Q15, Q15(2pi)-->205887

    //    /************************Speed PLL Coefficient*****************************/
    //    out->uwSpdPllKpPu = in->uwSpdPllKpPu;                                                       //Q14
    //    out->uwSpdPllKiPu = in->uwSpdPllKiPu;                                                       //Q14
    
    /************************Speed PLL Coefficient*****************************/
    uwMvcPu = ((ULONG)in->uwSpdPllWvcHz << 10) / in->uwFbHz; // Q10

    /* PLL Kp=M*w/sqrt(1+M^2) */
    ulDamper = (1 + in->uwSpdPllMcoef * in->uwSpdPllMcoef) << 8;              // Q8
    uwDamper = mth_slSqrt(ulDamper);                                          // Q4
    out->uwSpdPllKpPu = ((ULONG)in->uwSpdPllMcoef * uwMvcPu / uwDamper) << 8; // Q10-Q4+Q8=Q14

    /* PLL Ki=w^2*T_cnt_ctrl/sqrt(1+M^2) */
    out->uwSpdPllKiPu = ((((ULONG)uwMvcPu * out->uwCurTsPu) / uwDamper) * uwMvcPu) >> 17; // Q10+Q15-Q4+Q10-Q17=Q14
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
void spi_voMagneticDetection()
{
    UWORD MGL, MGH;
    MGL = gpio_input_bit_get(GPIOB, GPIO_PIN_0);
    MGH = gpio_input_bit_get(GPIOB, GPIO_PIN_1);
    if ((MGL != 0) || (MGH != 0))
    {
        spi_stResolverOut.blMagRangeFltFlg = TRUE;   // ! can not indicate magnet range fault
    }
    else
    {
        spi_stResolverOut.blMagRangeFltFlg = FALSE;
    }
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
SLONG spi_pvt_slSpdFbkLpfPu, spi_pvt_slSpdLpfTmpPu;
void  spi_voResolver(SPI_RESOLVER_COEF *coef, SPI_RESOLVER_OUT *out)
{
    UWORD uwSpiThetaTmpPu = 0, uwSpiThetaTmpPu2 = 0;
    SWORD swThetaErrPu = 0, swThetaCompPu = 0;
    SWORD swSpdTmpPu = 0, swSpdErrPu = 0;
    ULONG ulTmp1 = 0, ulTmp2 = 0;

    if (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) == SET)
    {
        uwSpiThetaTmpPu = ((SLONG)(spi_i2s_data_receive(SPI2))) >> 1;
        uwSpiThetaTmpPu &= 0x7FF8;
        uwSpiThetaTmpPu2 = uwSpiThetaTmpPu;
        
        swThetaCompPu = ((SLONG)out->swSpdLpfTmpPu * TLatency_TM) >> 10; // Q15, Consider decoding and SPI Latency:10us
        ulTmp1 = uwSpiThetaTmpPu + swThetaCompPu + out->swSpiThetaOffsetPu + cof_sl720DegreePu;
        uwSpiThetaTmpPu = ulTmp1 & 0x7FFF;

        ulTmp2 = uwSpiThetaTmpPu2 + cof_sl720DegreePu;
        uwSpiThetaTmpPu2 = ulTmp2 & 0x7FFF;
        out->uwSpiOrignData = uwSpiThetaTmpPu2;
        
        /* Calculate speed: Differentiation method */        
        swThetaErrPu = uwSpiThetaTmpPu - out->uwSpiThetaTmpZ1Pu;
        out->uwSpiThetaTmpZ1Pu = uwSpiThetaTmpPu;
        if (swThetaErrPu <= -cof_sl180DegreePu)
        {
            swThetaErrPu += cof_sl360DegreePu;
        }
        if (swThetaErrPu >= cof_sl180DegreePu)
        {
            swThetaErrPu -= cof_sl360DegreePu;
        }
        swSpdTmpPu = ((SLONG)swThetaErrPu * DIFF_COEF_TBC) >> 10;                                          // Q15
        
        /* Judge the correctness of spi position*/
        spi_pvt_slSpdLpfTmpPu = (SLONG)0x0277 * (swSpdTmpPu - out->swSpdLpfTmpPu) + spi_pvt_slSpdLpfTmpPu; // 50Hz    Q30
        out->swSpdLpfTmpPu = spi_pvt_slSpdLpfTmpPu >> 15;
        swSpdErrPu = out->swSpdLpfTmpPu - out->swSpdLpfTmpZ1Pu;
        out->swSpdLpfTmpZ1Pu = out->swSpdLpfTmpPu;
        if ((swSpdErrPu < USER_MOTOR_1000RPM2PU) && (swSpdErrPu > -USER_MOTOR_1000RPM2PU))     
        {
            out->uwSpiThetaPu = uwSpiThetaTmpPu;
            out->blSpiThetaFltFlg = FALSE;
        }
        else
        {
            out->blSpiThetaFltFlg = TRUE;
        }
    }
    else
    {
        out->blSpiThetaFltFlg = TRUE;
    }

     out->swSpdFbkPu = swSpdTmpPu;

     /*Calculate speed: PLL method*/
      SLONG slThetaErrPu,slThetaDeltaErrPu,slPllThetaPu;
      SLONG slKpTmpPu,slKitTmpPu,slPllSpdFbkPu;
      ULONG ulSpdPllKpPu,ulSpdPllKiPu;
  
      slThetaErrPu = (SLONG)out->uwSpiThetaPu - (out->uwPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) >> 10));  //Q15
      
      if (slThetaErrPu >= cof_sl180DegreePu)
      {
          slThetaErrPu-= cof_sl360DegreePu;
      }
      if(slThetaErrPu <= -cof_sl180DegreePu)
      {
          slThetaErrPu += cof_sl360DegreePu;
      }
      slThetaDeltaErrPu = slThetaErrPu - out->slThetaErrZ1Pu;
      out->slThetaErrZ1Pu = slThetaErrPu;
    
     /* Variable parameter PI，untested*/
//      if(out->swPllSpdFbkPu < cof_uw1000RpmPu - cof_uw200RpmPu)
//      {
//          ulSpdPllKpPu = coef->uwSpdPllKpPu; 
//          ulSpdPllKiPu = coef->uwSpdPllKiPu; 
//      }
//      else if((out->swPllSpdFbkPu > cof_uw1000RpmPu) && (out->swPllSpdFbkPu < cof_uw2000RpmPu - cof_uw200RpmPu))
//      {
//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 2; 
//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 2 * 2;      
//      }
//      else if((out->swPllSpdFbkPu > cof_uw2000RpmPu) && (out->swPllSpdFbkPu < cof_uw3000RpmPu - cof_uw200RpmPu))
//      {
//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 4; 
//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 4 * 4;        
//      }
//      else if((out->swPllSpdFbkPu > cof_uw3000RpmPu) && (out->swPllSpdFbkPu < cof_uw4000RpmPu - cof_uw200RpmPu))
//      {
//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 8; 
//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 8 * 8;        
//      }
//      else if (out->swPllSpdFbkPu > cof_uw4000RpmPu)
//      {
//          ulSpdPllKpPu = coef->uwSpdPllKpPu * 16; 
//          ulSpdPllKiPu = coef->uwSpdPllKiPu * 16 * 16; 
//      }
    
      ulSpdPllKpPu = coef->uwSpdPllKpPu; 
      ulSpdPllKiPu = coef->uwSpdPllKiPu; 
      
      slKpTmpPu = slThetaDeltaErrPu * ulSpdPllKpPu;   //Q15+Q14=Q29
      slKitTmpPu = slThetaErrPu * ulSpdPllKiPu;
      slPllSpdFbkPu = slKpTmpPu + slKitTmpPu + out->slPllSpdFbkPu;
      if(slPllSpdFbkPu >= 0x20000000)
      {
          slPllSpdFbkPu = 0x20000000-1; 																	                    //Q29
      }
      if(slPllSpdFbkPu <= -0x20000000)
      {
          slPllSpdFbkPu = -0x20000000; 																			            //Q29
      }
      out->slPllSpdFbkPu = slPllSpdFbkPu;                                                              //Q29
      out->swPllSpdFbkPu = slPllSpdFbkPu >> 14;                                                   //Q15
  
      slPllThetaPu = out->slPllThetaPu + (((SLONG)out->swPllSpdFbkPu * coef->uwCurTs) << 4);    //Q15+Q10+Q4=Q29
      if(slPllThetaPu >= 0x20000000)
      {
          slPllThetaPu -= 0x20000000;
      }
      if(slPllThetaPu < 0)
      {
          slPllThetaPu += 0x20000000;
      }
      out->slPllThetaPu = slPllThetaPu;
      out->uwPllThetaPu = slPllThetaPu >> 14;        //Q15 = Q29 - Q14
      
//      spi_pvt_slSpdFbkLpfPu = (SLONG)0x00FF * (out->swPllSpdFbkPu - out->swSpdFbkLpfPu) + spi_pvt_slSpdFbkLpfPu;         //20Hz    Q30
//      out->swSpdFbkLpfPu = spi_pvt_slSpdFbkLpfPu >> 15;
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
void spi_voResolverLock()
{
    /* CS signal enable */
    IO_SPI2_NSS_ENABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
    {}
    spi_i2s_data_transmit(SPI2,0x0550);
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TRANS) != RESET)
    {}
    /* CS signal disable */
    IO_SPI2_NSS_DISABLE;    
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
void spi_voReadWriteSeneorReg(void)
{
    UWORD uwReadBCTReg = 0, uwReadETXY = 0;
    UWORD uwWriteBCTReg = 0, uwWriteETXY = 0, uwWriteRD = 0;
    UWORD SPI_DelayCnt1 = 0, SPI_DelayCnt2 = 0;

    /* Read the BCT register value */
    IO_SPI2_NSS_ENABLE; /*!< CS signal enable,for data update */
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
    {}
    spi_i2s_data_transmit(SPI2, 0x4200); //comp value
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TRANS) != RESET)
    {}
    IO_SPI2_NSS_DISABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
    {}
    uwReadBCTReg = spi_i2s_data_receive(SPI2); /*!< Read the first time */
    IO_SPI2_NSS_ENABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
    {}
    spi_i2s_data_transmit(SPI2, 0x0000); /*!< Write the second time */
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TRANS) != RESET)
    {}
    IO_SPI2_NSS_DISABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
    {}
    uwReadBCTReg = spi_i2s_data_receive(SPI2); /*!< Read the BCT register value */

    /* Read the ETX ETY value */
    IO_SPI2_NSS_ENABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
    {}
    spi_i2s_data_transmit(SPI2, 0x4300); //comp direction 
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TRANS) != RESET)
    {}
    IO_SPI2_NSS_DISABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
    {}
    uwReadETXY = spi_i2s_data_receive(SPI2); /*!< Read the first time */
    IO_SPI2_NSS_ENABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
    {}
    spi_i2s_data_transmit(SPI2, 0x0000); /*!< Write the second time */
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_TRANS) != RESET)
    {}
    IO_SPI2_NSS_DISABLE;
    while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
    {}
    uwReadETXY = spi_i2s_data_receive(SPI2); /*!< Read the ETX ETY value */
    if (uwReadBCTReg == 0x3000 && uwReadETXY == 0x0100)
    {
        spi_blReadRegCorrectFlg = TRUE;
    }
    else
    {
        spi_blReadRegCorrectFlg = FALSE;
    }

    /* Write MA702 BCT, EXY, RD Register */
    if ((spi_blReadRegCorrectFlg == FALSE) && (spi_blWriteRegFinishFlg == FALSE))
    //if (spi_blWriteRegFinishFlg == FALSE)
    {
        /* Write and Read BCT value*/
        IO_SPI2_NSS_ENABLE;   // Data update
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)// Discontinuous transmission, can not indicate send complete
        {}
        spi_i2s_data_transmit(SPI2, 0x8230); // LSB,BCT=48
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteBCTReg = spi_i2s_data_receive(SPI2);       
        IO_SPI2_NSS_DISABLE;
        /* Delay at least 22ms */
        while (SPI_DelayCnt2 < 20)
        {
            SPI_DelayCnt1++;
            if (SPI_DelayCnt1 == 10000)
            {
                SPI_DelayCnt2++;
                SPI_DelayCnt1 = 0;
            }
        }
        IO_SPI2_NSS_ENABLE;
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
        {}
        spi_i2s_data_transmit(SPI2, 0x0000);
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteBCTReg = spi_i2s_data_receive(SPI2);
        IO_SPI2_NSS_DISABLE;
        /* Write and Read ETX or ETY */
        IO_SPI2_NSS_ENABLE;
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
        {}
        spi_i2s_data_transmit(SPI2, 0x8301);
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteETXY = spi_i2s_data_receive(SPI2);
        IO_SPI2_NSS_DISABLE;
        /* Delay at least 22ms */
        SPI_DelayCnt2 = 0;
        while (SPI_DelayCnt2 < 20)
        {
            SPI_DelayCnt1++;
            if (SPI_DelayCnt1 == 10000)
            {
                SPI_DelayCnt2++;
                SPI_DelayCnt1 = 0;
            }
        }
        IO_SPI2_NSS_ENABLE;
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
        {}
        spi_i2s_data_transmit(SPI2, 0x0000);
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteETXY = spi_i2s_data_receive(SPI2);
        IO_SPI2_NSS_DISABLE;
        
        /* Write and Read RD value*/
        IO_SPI2_NSS_ENABLE;
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
        {}
        spi_i2s_data_transmit(SPI2, 0x8980); // RD=1,Counterclockwise,8980; RD=0,Clockwise,8900
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteRD = spi_i2s_data_receive(SPI2);
        IO_SPI2_NSS_DISABLE;
        /* Delay at least 22ms */
        SPI_DelayCnt2 = 0;
        while (SPI_DelayCnt2 < 20)
        {
            SPI_DelayCnt1++;
            if (SPI_DelayCnt1 == 10000)
            {
                SPI_DelayCnt2++;
                SPI_DelayCnt1 = 0;
            }
        }
        IO_SPI2_NSS_ENABLE;
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_TBE) != SET)
        {}
        spi_i2s_data_transmit(SPI2, 0x0000);
        while (spi_i2s_flag_get(SPI2, SPI_FLAG_RBNE) != SET)
        {}
        uwWriteRD = spi_i2s_data_receive(SPI2);
        IO_SPI2_NSS_DISABLE;
        
        if (uwWriteBCTReg == 0x3000 && uwWriteETXY == 0x0100 && uwWriteRD == 0x8000) // MSB
        {
            spi_blWriteRegFinishFlg = TRUE; // Need stored in EEPROM
        }
        else
        {
            spi_blWriteRegFinishFlg = FALSE;
        }
    }
}
/*************************************************************************
 Local Functions (N/A)
*************************************************************************/

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