/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            asr.c
 Partner Filename:    asr.h
 Description:         Automatic speed regulator
 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 _ALIGN_C_
#define _ALIGN_C_
#endif
/************************************************************************
 Included File:
*************************************************************************/

#include "alignstartup.h"
#include "CodePara.h"
#include "syspar.h"
#include "glbcof.h"
#include "spdctrmode.h"
/************************************************************************
 Constant Table:
*************************************************************************/
void align_voInitPos(ALIGN_IN *in, ALIGN_COF *coef, ALIGN_OUT *out)
{
    out->ulStatCt = in->ulStatCt;
    if (out->ulStatCt < coef->ulAlignRampTbcCt)
    {
        out->slIdRefPu += coef->ulAlignCurIncPerTbcPu;
        out->swIdRefPu = out->slIdRefPu >> 15;
        if (out->swIdRefPu > (SWORD)coef->uwDragCurPu)
        {
            out->swIdRefPu = (SWORD)coef->uwDragCurPu;
            out->slIdRefPu = out->swIdRefPu << 15;
        }
    }
    else if (out->ulStatCt >= (coef->ulAlignRampTbcCt + coef->ulAlignHoldTbcCt))
    {
        // coef->uwSPIreadOnceCt++;
        // if (coef->uwSPIreadOnceCt == 3)
        // {
        //     // spi_stResolverOut.swSpiThetaOffsetPu = (SWORD)align_stOut.uwAngRefPu - spi_stResolverOut.uwSpiThetaPu; //q15
        //     spi_stResolverOut.swSpiThetaOffsetPu = (SWORD)align_stOut.uwAngRefPu - spi_stResolverOut.uwSpiOrignData; // q15
        //     if(cp_stFlg.SpiOffsetFirstSetFlg == 0)
        //     {
        //         spi_stResolverOut.swSpiThetaOffsetOrignPu = spi_stResolverOut.swSpiThetaOffsetPu;
        //     }
        // }
    }
    out->slIqRefPu = 0;
    out->swIqRefPu = 0;
    out->slAngManuPu = coef->slAlignAngInit;
    out->uwAngManuPu = coef->slAlignAngInit;
    out->uwAngRefPu = out->uwAngManuPu;
}

void align_voStartUp(ALIGN_IN *in, ALIGN_COF *coef, ALIGN_OUT *out)
{
    if (out->ulStatCt < coef->ulDragSpdRampTbcCt)
    {
        out->ulStatCt = in->ulStatCt;
        out->slDragSpdRefPu += (SLONG)in->swRotateDir * coef->ulDragSpdIncPerTbcPu; // Q29
        out->slDragSpdPu = out->slDragSpdRefPu >> 14;
        out->blStartUpOvrFlg = FALSE;
    }
    else if(out->ulStatCt < coef->ulDragSpdRampTbcCt + 5000)
    {
        out->ulStatCt = in->ulStatCt;
    }
    else
    {
        out->ulStatCt = coef->ulDragSpdRampTbcCt + 5000;
        out->blStartUpOvrFlg = TRUE;
    }
//    else
//    {
//        out->ulStatCt = coef->ulDragSpdRampTbcCt;
//        out->blStartUpOvrFlg = TRUE;
//    }
    


    if (out->slDragSpdPu > (SLONG)coef->uwDragSpdPu)
    {
        out->slDragSpdPu = (SLONG)coef->uwDragSpdPu;
    }
    else if (out->slDragSpdPu < -(SLONG)coef->uwDragSpdPu)
    {
        out->slDragSpdPu = -(SLONG)coef->uwDragSpdPu;
    }

    out->slAngManuPu = out->uwAngManuPu;

    out->slAngManuPu += ((SQWORD)out->slDragSpdPu * TBC_TM >> 10);

    if (out->slAngManuPu >= cof_sl360DegreePu)
    {
        out->slAngManuPu -= cof_sl360DegreePu;
    }
    else if (out->slAngManuPu < 0)
    {
        out->slAngManuPu += cof_sl360DegreePu;
    }

    out->uwAngManuPu = (UWORD)out->slAngManuPu;
    out->uwAngRefPu = out->uwAngManuPu;
    out->swIdRefPu = 0;
    out->swIqRefPu = in->swRotateDir * coef->uwDragCurPu;
    out->slIqRefPu = (((SLONG)out->swIqRefPu)<<15);

}

void align_voOpen2Clz(ALIGN_IN *in, ALIGN_COF *coef, ALIGN_OUT *out)
{
    SWORD swRotorAngleErrorPu; // Q15
//    if (!out->blCurSwitchOvrFlg)
//    {
//        out->swIdRefPu -= coef->ulOpen2ClzCurIncPerTbcPu; // Q14
//        if (out->swIdRefPu <= cof_swIdMaxPu)
//        {
//            out->swIdRefPu = cof_swIdMaxPu; // Q14
//            out->blCurSwitchOvrFlg = TRUE;
//        }
//    }
//    out->swIqRefPu = in->swCurRefrompu;

    if (!out->blCurSwitchOvrFlg)
    {
        if(scm_swSpdRefPu > 0)
        {
            if(out->swIqRefPu > in->swCurRefrompu)
            {
                out->swIdRefPu =0;
                out->slIqRefPu -= (coef->ulAlignCurIncPerTbcPu)*6;  //1310720;
                out->swIqRefPu = out->slIqRefPu >> 15;
            }
            else
            {
                out->swIdRefPu =0;
                out->blCurSwitchOvrFlg = TRUE;
                out->slIqRefPu = ((SLONG)in->swCurRefrompu<<15);
                out->swIqRefPu = out->slIqRefPu >> 15;
            }
        }
        else
        {
            if(out->swIqRefPu < in->swCurRefrompu)
            {
                out->swIdRefPu =0;
                out->slIqRefPu += (coef->ulAlignCurIncPerTbcPu)*6;  //1310720;
                out->swIqRefPu = out->slIqRefPu >> 15;
            }
            else
            {
                out->blCurSwitchOvrFlg = TRUE;
                out->swIdRefPu =0;
                out->slIqRefPu = ((SLONG)in->swCurRefrompu<<15);
                out->swIqRefPu = out->slIqRefPu >> 15;
            }
        }
    }
    else
    {
        out->swIdRefPu =0;
        out->swIqRefPu = in->swCurRefrompu;
    }

    if (!out->blAngSwitchOvrFlg)
    {
        out->slAngManuPu += ((SQWORD)out->slDragSpdPu * TBC_TM >> 10);

        if (out->slAngManuPu >= cof_sl360DegreePu)
        {
            out->slAngManuPu -= cof_sl360DegreePu;
        }
        else if (out->slAngManuPu < 0)
        {
            out->slAngManuPu += cof_sl360DegreePu;
        }
        out->uwAngManuPu = (UWORD)out->slAngManuPu;

        if (++out->uwAngSwitchK >= 256)
        {
            out->uwAngSwitchK = 256;
            out->blAngSwitchOvrFlg = TRUE;
        }

        swRotorAngleErrorPu = (SLONG)in->uwObsElecThetaPu - (SLONG)out->uwAngManuPu;
        if (swRotorAngleErrorPu > cof_sl180DegreePu)
        {
            out->uwAngRefPu = ((ULONG)in->uwObsElecThetaPu * out->uwAngSwitchK >> 8) +
                              ((out->uwAngManuPu + (ULONG)cof_sl360DegreePu) * (256 - out->uwAngSwitchK) >> 8);
        }
        else if ((swRotorAngleErrorPu + cof_sl180DegreePu) < 0)
        {
            out->uwAngRefPu = (((ULONG)in->uwObsElecThetaPu + (ULONG)cof_sl360DegreePu) * out->uwAngSwitchK >> 8) +
                              ((ULONG)out->uwAngManuPu * (256 - out->uwAngSwitchK) >> 8);
        }
        else
        {
            out->uwAngRefPu =
                ((ULONG)in->uwObsElecThetaPu * out->uwAngSwitchK >> 8) + ((ULONG)out->uwAngManuPu * (256 - out->uwAngSwitchK) >> 8); // Q15
        }

        if (out->uwAngRefPu >= cof_sl360DegreePu)
        {
            out->uwAngRefPu -= cof_sl360DegreePu;
        }
    }
    else
    {
        out->uwAngRefPu = in->uwObsElecThetaPu;
    }
}

void align_voCoef(void)
{
    // Align Parameters
    align_stCoef.uwAlignCurPu = CUR_AP2PU(cp_stControlPara.swAlignCurAp);
    align_stCoef.ulAlignRampTbcCt = TBC_MS2CT(cp_stControlPara.swAlignRampTMms);
    align_stCoef.ulAlignHoldTbcCt = TBC_MS2CT(cp_stControlPara.swAlignHoldTMms);
    align_stCoef.uwAlignCurPu = CUR_AP2PU(cp_stControlPara.swAlignCurAp);

    if (align_stCoef.ulAlignRampTbcCt == 0)
    {
        align_stCoef.ulAlignCurIncPerTbcPu = 1;
    }
    else
    {
        align_stCoef.ulAlignCurIncPerTbcPu = ((ULONG)align_stCoef.uwAlignCurPu << 15) / align_stCoef.ulAlignRampTbcCt; // Q29
        if (align_stCoef.ulAlignCurIncPerTbcPu == 0)
        {
            align_stCoef.ulAlignCurIncPerTbcPu = 1;
        }
    }

    align_stCoef.slAlignAngInit = ANG_DEG2PU(cp_stControlPara.swAlignAngInitDeg);

    // Open Drag Parameters
    align_stCoef.uwDragCurPu = CUR_AP2PU(cp_stControlPara.swDragCurAp); // Q14
    align_stCoef.uwDragSpdPu = SPD_HZ2PU(cp_stControlPara.swDragSpdHz); // Q15
    align_stCoef.ulDragSpdRampTbcCt = TBC_MS2CT(cp_stControlPara.swDragSpdRampTMms);

    if (align_stCoef.ulDragSpdRampTbcCt == 0)
    {
        align_stCoef.ulDragSpdIncPerTbcPu = 1;
    }
    else
    {
        align_stCoef.ulDragSpdIncPerTbcPu = ((ULONG)align_stCoef.uwDragSpdPu  << 14) /  align_stCoef.ulDragSpdRampTbcCt; // Q29
        if (align_stCoef.ulDragSpdIncPerTbcPu == 0)
        {
            align_stCoef.ulDragSpdIncPerTbcPu = 1;
        }
    }

    // Open to Close Parameters
    align_stCoef.ulOpen2ClzCurRampTbcCt = TBC_MS2CT(cp_stControlPara.swOpen2ClzRampTMms);
    if (align_stCoef.ulOpen2ClzCurRampTbcCt == 0)
    {
        align_stCoef.ulOpen2ClzCurIncPerTbcPu = 1;
    }
    else
    {
        align_stCoef.ulOpen2ClzCurIncPerTbcPu = (ULONG)align_stCoef.uwDragCurPu / align_stCoef.ulOpen2ClzCurRampTbcCt;
        if (align_stCoef.ulOpen2ClzCurIncPerTbcPu == 0)
        {
            align_stCoef.ulOpen2ClzCurIncPerTbcPu = 1;
        }
    }
}

void align_voInit(void)
{
    align_stOut.ulStatCt = 0;
    align_stOut.uwAngSwitchK = 0;
    align_stOut.slIdRefPu = 0; // Q26, Output voltage feedback
    align_stOut.swIdRefPu = 0; // Q14, Error recorder
    align_stOut.slIqRefPu = 0; // Q26, Output voltage feedback
    align_stOut.swIqRefPu = 0; // Q14, Error recorder
    align_stOut.slDragSpdRefPu = 0;
    align_stOut.slDragSpdPu = 0;
    align_stOut.slAngManuPu = 0;
    align_stOut.uwAngManuPu = 0;
    align_stOut.uwAngRefPu = 0;
    align_stOut.blStartUpOvrFlg = FALSE;
    align_stOut.blCurSwitchOvrFlg = FALSE;
    align_stOut.blAngSwitchOvrFlg = FALSE;

    align_voCoef();
}