/************************************************************************
 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 :           STM32F30x
*************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************
*************************************************************************
 Revising History (ECL of this file):
 M0_20170410, by liyue, create 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"
#include "spi_master.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.swSpdTmpPu = 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.slSpdFbkPu = 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()
{
    // UBYTE MGL, MGH;
    // MGL = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_0);
    // MGH = GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_1);
    // if ((MGL != 0) || (MGH != 0))
    // {
    //     blMagRangeFltFlg = TRUE;
    // }
}
/*************************************************************************
 Function:
 Description:
 Call by:
 Input Variables:
 Output/Return Variables:
 Subroutine Call:
 Reference:
*************************************************************************/
SLONG spi_pvt_slSpdFbkLpfPu, spi_pvt_slSpdLpfTmpPu;
UWORD TstCnt = 0;
UWORD TstThetaZ1Pu;
SWORD TstSpdPu;
UWORD DATA = 0;
UWORD LASTDATA = 0;
SWORD DATAERROR = 0;
SWORD SPICNT = 0;
UWORD THETACHANGE = 0;
UWORD THETACHANGELast = 0;
void  spi_voResolver(SPI_RESOLVER_COEF *coef, SPI_RESOLVER_OUT *out)
{
    UWORD uwSpiThetaTmpPu, uwSpiThetaTmpPu2;
    SWORD swThetaErrPu, swThetaCompPu;
    SWORD swSpdErrPu;
    ULONG ulTmp1, ulTmp2;

    if (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) == SET)
    {
        uwSpiThetaTmpPu = ((SLONG)(SPI_I2S_ReceiveData(SPI3))) >> 1;
        uwSpiThetaTmpPu &= 0x7FF8;
        uwSpiThetaTmpPu2 = uwSpiThetaTmpPu;
        ///////////////////////////////////////////////////////////////////////////////////
        //        UWORD FLAG = 0;
        //        SWORD JUDGEError = 0;
        //        LASTDATA = DATA;
        //        JUDGEError = uwSpiThetaTmpPu - out->swSpiThetaOffset2Pu;
        //        if(JUDGEError < 0)
        //        {
        //          FLAG = 1;
        //        }
        //        if(JUDGEError >= 0 )
        //        {
        //          FLAG = 0;
        //        }
        //        DATA = (ULONG)uwSpiThetaTmpPu + FLAG*32768 - out->swSpiThetaOffset2Pu;
        //        DATAERROR = ((SWORD)DATA - (SWORD)LASTDATA);
        //        if(DATAERROR < -16384)
        //        {
        //          SPICNT ++;
        //          if(SPICNT == 5)
        //          {
        //            SPICNT=0;
        //          }
        //        }
        //
        //        if(DATAERROR > 16384)
        //        {
        //          SPICNT--;
        //          if(SPICNT<0)
        //          {
        //            SPICNT = 4;
        //          }
        //        }
        //
        //        UWORD PERCENT = 0;
        //        ULONG thetaFive2Seven = 0;
        //        PERCENT = ((ULONG)72 * uwSpiThetaTmpPu / 32768) + SPICNT * 72;
        //        thetaFive2Seven = ((SQWORD)32768 * 7 / 360) * PERCENT;                   //((SQWORD)32768 * 7 * PERCENT / (360 * 5))
        //        while(thetaFive2Seven > 32768)
        //        {
        //          thetaFive2Seven = thetaFive2Seven - 32768;
        //        }
        //        THETACHANGELast = THETACHANGE;
        //        THETACHANGE = (UWORD)thetaFive2Seven;
        /////////////////////////////////////////////////////////////////////////////////////
        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;
        /* Judge the correctness of data*/
        swThetaErrPu = uwSpiThetaTmpPu - out->uwSpiThetaTmpZ1Pu;
        out->uwSpiThetaTmpZ1Pu = uwSpiThetaTmpPu;
        if (swThetaErrPu <= -cof_sl180DegreePu)
        {
            swThetaErrPu += cof_sl360DegreePu;
        }
        if (swThetaErrPu >= cof_sl180DegreePu)
        {
            swThetaErrPu -= cof_sl360DegreePu;
        }
        //        out->swSpdTmpPu = swThetaErrPu * 14629>>12; // Q15, f_tbs/fbase*2^12=14629
        //        out->swSpdTmpPu = swThetaErrPu * 58516>>12; // Q15, f_tbc/fbase*2^12=58516,f_tbc=10000;
        out->swSpdTmpPu = ((SLONG)swThetaErrPu * DIFF_COEF_TBC) >> 10;                                          // Q15
        spi_pvt_slSpdLpfTmpPu = (SLONG)0x0277 * (out->swSpdTmpPu - out->swSpdLpfTmpPu) + spi_pvt_slSpdLpfTmpPu; // 50Hz    Q30
        out->swSpdLpfTmpPu = spi_pvt_slSpdLpfTmpPu >> 15;
        swSpdErrPu = out->swSpdLpfTmpPu - out->swSpdLpfTmpZ1Pu;
        out->swSpdLpfTmpZ1Pu = out->swSpdLpfTmpPu;
        //        spi_pvt_slSpdLpfTmpPu = 3*out->swSpdTmpPu/10 + 7*out->swSpdLpfTmpPu/10;
        //        out->swSpdLpfTmpZ1Pu =  spi_pvt_slSpdLpfTmpPu ;

        if ((swSpdErrPu < 6515) && (swSpdErrPu > -6515)) //   20deg electrical angle, ? rpm
        {
            out->uwSpiThetaPu = uwSpiThetaTmpPu;
        }
        else
        {
            blSpiThetaFltFlg = TRUE;
        }
    }
    else
    {
        blSpiThetaFltFlg = TRUE;
    }
    //    TstCnt ++;
    //    if(TstCnt > 3)
    //    {
    //        TstCnt =0;
    //        TstSpdPu = ((SLONG)(out->uwSpiThetaPu - TstThetaZ1Pu) * DIFF_COEF_TBS) >>10;
    //        TstThetaZ1Pu = out->uwSpiThetaPu;
    //    }
    out->swSpdFbkPu = out->swSpdTmpPu;

    //   /*Calculate speed: PLL method*/
    //    out->slThetaErrPu = (SLONG)out->uwSpiThetaPu - (out->uwPllThetaPu + (((SLONG)out->swSpdFbkLpfPu * TBC_TM) >> 10));
    //    if (out->slThetaErrPu >= cof_sl180DegreePu)
    //    {
    //        out->slThetaErrPu -= cof_sl360DegreePu;
    //    }
    //    if(out->slThetaErrPu <= -cof_sl180DegreePu)
    //    {
    //        out->slThetaErrPu += cof_sl360DegreePu;
    //    }
    //    out->slThetaDeltaErrPu = out->slThetaErrPu - out->slThetaErrZ1Pu;
    //    out->slThetaErrZ1Pu = out->slThetaErrPu;
    //    slKpTmpPu = out->slThetaDeltaErrPu * coef->uwSpdPllKpPu; 									//Q15+Q10=Q25
    //    slKitTmpPu = out->slThetaErrPu * coef->uwSpdPllKiPu;
    //    slSpdFbkPu = slKpTmpPu + slKitTmpPu + out->slSpdFbkPu;
    //    /* Limit the speed value to avoid overflow */
    //    if(slSpdFbkPu >= 0x20000000)
    //    {
    //        slSpdFbkPu = 0x20000000-1; 																	                    //Q29
    //    }
    //    if(slSpdFbkPu <= -0x20000000)
    //    {
    //        slSpdFbkPu = -0x20000000; 																			            //Q29
    //    }
    //    out->slSpdFbkPu = slSpdFbkPu;                                                              //Q29
    //    //Q29 out->swSpdFbkPu = slSpdFbkPu >> 14;                                                   //Q15
    //
    //    slPllThetaPu = out->slPllThetaPu + (((SLONG)out->swSpdFbkPu * 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->swSpdFbkPu - 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()
{
    /* SPI1 resolver enable */
    //    SPI_Cmd(SPI3,ENABLE);
    /* CS signal enable */
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x0550);
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) == SET)
    {}
    //    SPI_Cmd(SPI3,DISABLE);
    IO_SPI3_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;
    _Bool  blReadCorrectFlg = FALSE, blWriteFinishFlg = FALSE;
    UWORD SPI_DelayCnt1 = 0, SPI_DelayCnt2 = 0;

    //    /* SPI1 resolver enable */
    //    SPI_Cmd(SPI3,ENABLE);
    /*Write command of read the BCT register value */
    IO_SPI3_NSS_ENABLE; /*!< CS signal enable,for data update*/
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x4200); // comp value
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    uwReadBCTReg = SPI_I2S_ReceiveData(SPI3); /*!< Read the first time*/
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x0000); /*!< Write the second time*/
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    uwReadBCTReg = SPI_I2S_ReceiveData(SPI3); /*!< Read the BCT register value*/

    /*Write command of read the ETX ETY value */
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x4300); // comp direction
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    uwReadETXY = SPI_I2S_ReceiveData(SPI3); /*!< Read the first time*/
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x0000); /*!< Write the second time*/
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    uwReadETXY = SPI_I2S_ReceiveData(SPI3); /*!< Read the ETX ETY value*/
    if (uwReadBCTReg == 0x3000 && uwReadETXY == 0x0100)
    {
        blReadCorrectFlg = TRUE;
    }
    else
    {
        blReadCorrectFlg = FALSE;
    }

    if ((blReadCorrectFlg == FALSE) && (blWriteFinishFlg == FALSE))
    {
        /* Write and Read BCT value*/
        IO_SPI3_NSS_ENABLE;                                          // Data update
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET) // Discontinuous transmission, can not indicate send complete
        {}
        SPI_I2S_SendData(SPI3, 0x8230); // LSB,BCT=48
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
        {}
        IO_SPI3_NSS_DISABLE; // Data update
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
        {}
        uwWriteBCTReg = SPI_I2S_ReceiveData(SPI3);
        /*delay 22ms*/
        while (SPI_DelayCnt2 < 20)
        {
            SPI_DelayCnt1++;
            if (SPI_DelayCnt1 == 10000)
            {
                SPI_DelayCnt2++;
                SPI_DelayCnt1 = 0;
            }
        }
        IO_SPI3_NSS_ENABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
        {}
        SPI_I2S_SendData(SPI3, 0x0000);
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
        {}
        IO_SPI3_NSS_DISABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
        {}
        uwWriteBCTReg = SPI_I2S_ReceiveData(SPI3);

        /* Write and Read ETX or ETY */
        IO_SPI3_NSS_ENABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
        {}
        SPI_I2S_SendData(SPI3, 0x8301);
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
        {}
        IO_SPI3_NSS_DISABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
        {}
        uwWriteETXY = SPI_I2S_ReceiveData(SPI3);
        /*delay 22ms*/
        SPI_DelayCnt2 = 0;
        while (SPI_DelayCnt2 < 20)
        {
            SPI_DelayCnt1++;
            if (SPI_DelayCnt1 == 10000)
            {
                SPI_DelayCnt2++;
                SPI_DelayCnt1 = 0;
            }
        }
        IO_SPI3_NSS_ENABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
        {}
        SPI_I2S_SendData(SPI3, 0x0000);
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
        {}
        IO_SPI3_NSS_DISABLE;
        while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
        {}
        uwWriteETXY = SPI_I2S_ReceiveData(SPI3);
        if (uwWriteBCTReg == 0x3000 && uwWriteETXY == 0x0100) // MSB,BCT=48
        {
            blWriteFinishFlg = TRUE; // Stored in EEPROM
        }
        else
        {
            blWriteFinishFlg = FALSE;
        }
    }
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;

    /* Write and Read RD value*/
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x8980); // RD=1,Counterclockwise 8980; RD=0,Clockwise 8900
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    /*delay 22ms*/
    SPI_DelayCnt2 = 0;
    while (SPI_DelayCnt2 < 20)
    {
        SPI_DelayCnt1++;
        if (SPI_DelayCnt1 == 10000)
        {
            SPI_DelayCnt2++;
            SPI_DelayCnt1 = 0;
        }
    }
    IO_SPI3_NSS_ENABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_TXE) != SET)
    {}
    SPI_I2S_SendData(SPI3, 0x0000);
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_BSY) != RESET)
    {}
    IO_SPI3_NSS_DISABLE;
    while (SPI_I2S_GetFlagStatus(SPI3, SPI_I2S_FLAG_RXNE) != SET)
    {}
    //    SPI_Cmd(SPI3,DISABLE);
}

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

/*************************************************************************
 Copyright (c) 2018 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!
*************************************************************************/