/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            brake.c
 Partner Filename:    brake.h
 Description:         The brake process
 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):
WLBDM_M4_SR_20180831-new FSM1.1, by mz, create this file;
************************************************************************/

/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#ifndef _BRAKE_C_
#define _BRAKE_C_
#endif
/************************************************************************
 Included File:
*************************************************************************/
#include "brake.h"
#include "glbcof.h"

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: cvb_voBrakeInit;
 Description: constant voltage brake initial;
 Call by: functions in main loop;
 Input Variables: N/A;
 Output/Return Variables: N/A;
 Subroutine Call: N/A;
 Reference: N/A;
****************************************************************/
void cvb_voBrakeInit(void)
{
    cvb_stBrakeOut.swIqRefPu = 0;
    cvb_stBrakeOut.swIqLimPu = 0;
    cvb_stBrakeOut.swIdRefPu = 0;
    cvb_stBrakeOut.uwAngelPu = 0;
    cvb_stBrakeOut.blCvbFlg = FALSE;
}
/***************************************************************
 Function: cvb_voBrakeCoef;
 Description:  constant voltage brake coefficient initial;
 Call by: functions in main loop;
 Input Variables: BRAKE_COFIN;
 Output/Return Variables: BRAKE_COF;
 Subroutine Call: N/A;
 Reference: N/A;
****************************************************************/
void cvb_voBrakeCoef(BRAKE_COFIN *in, BRAKE_COF *coef)
{
    UWORD uwDeltaLagVdcPu;

    if (in->uwVdcCvbVt > 6000)
    {
        in->uwVdcCvbVt = 6000;
    }
    else if (in->uwVdcCvbVt < 100)
    {
        in->uwVdcCvbVt = 100;
    }

    if (in->swIqRefMaxAp < 0)
    {
        in->swIqRefMaxAp = 0;
    }

    if (in->swIdRefMaxAp > 0)
    {
        in->swIdRefMaxAp = 0;
    }

    if (in->swIdRefMinAp > 0)
    {
        in->swIdRefMinAp = 0;
    }

    if (in->swIdRefMinAp > in->swIdRefMaxAp)
    {
        in->swIdRefMinAp = in->swIdRefMaxAp;
    }

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

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

    coef->uwVdcCvbPu = ((ULONG)in->uwVdcCvbVt << 14) / in->uwVBaseVt; // Q14
    coef->uwVdcLagAngelPu = ((ULONG)(in->uwVdcCvbVt + 10) << 14) / in->uwVBaseVt;
    coef->uwVdcStartCvbPu = ((ULONG)(in->uwVdcCvbVt - 10) << 14) / in->uwVBaseVt;
    uwDeltaLagVdcPu = coef->uwVdcLagAngelPu - coef->uwVdcCvbPu; // Q14

    coef->swIqLimMaxPu = ((SLONG)in->swIqRefMaxAp << 14) / in->uwIBaseAp; // Q14

    coef->swIdRefMaxPu = ((SLONG)in->swIdRefMaxAp << 14) / in->uwIBaseAp; // Q14
    coef->swIdRefMinPu = ((SLONG)in->swIdRefMinAp << 14) / in->uwIBaseAp; // Q14

    coef->uwKAnglePu = ((((SLONG)5 << 15) / 360) << 7) / uwDeltaLagVdcPu; // Q8

    coef->uwLowSpdPu = ((SLONG)(in->uwLowSpdRpm) << 15) / 60 * in->uwMotorPairs / in->uwFBaseHz;

    coef->swKcvb = 17 << 9;
    coef->swIdDrpPu = 8;
    coef->swIdRcyPu = 8;
}
/***************************************************************
 Function: cvb_voBrake;
 Description: constant voltage brake
 Call by: functions in TBC;
 Input Variables: BRAKE_IN, BRAKE_COF;
 Output/Return Variables: BRAKE_OUT;
 Subroutine Call: N/A;
 Reference: N/A;
****************************************************************/
void cvb_voBrake(BRAKE_IN *in, BRAKE_COF *coef, BRAKE_OUT *out)
{
    SWORD swDelta;

    /*set Iq output limit for Constant Voltage Break*/
    if (in->uwVdcLpfPu >= coef->uwVdcStartCvbPu)
    {
        out->swIqLimPu = (SWORD)in->uwVdcLpfPu - coef->uwVdcCvbPu;
        out->swIqLimPu = ((SLONG)out->swIqLimPu * coef->swKcvb) >> 9; // Q14+Q9-Q9=Q14
    }
    else
    {
        out->swIqLimPu = -coef->swIqLimMaxPu;
    }
    if (out->swIqLimPu > 0)
    {
        out->swIqLimPu = 0;
    }
    out->swIqLimPu = -out->swIqLimPu;

    if (out->swIqLimPu > coef->swIqLimMaxPu)
    {
        out->swIqLimPu = coef->swIqLimMaxPu;
    }
    if ((in->uwVdcLpfPu < coef->uwVdcCvbPu) && (coef->swKcvb > 16384) && (out->swIqLimPu == 0)) // avoid overflow
    {
        out->swIqLimPu = coef->swIqLimMaxPu;
    }

    /*set Id output for Constant Voltage Break*/
    if (in->uwSpdLpfAbsPu < coef->uwLowSpdPu)
    {
        out->swIdRefPu = in->swIdRefPu;
        out->swIdRefPu += coef->swIdRcyPu;
    }
    else
    {
        out->swIdRefPu = in->swIdRefPu;
        out->swIdRefPu -= coef->swIdDrpPu;
    }

    /*set Angle output for Constant Voltage Break*/
    if (in->uwVdcLpfPu < coef->uwVdcCvbPu)
    {
        out->uwAngelPu = in->uwAngelPu;
        swDelta = 0;
    }
    else if (in->uwVdcLpfPu < coef->uwVdcLagAngelPu)
    {
        swDelta = ((SLONG)(in->uwVdcLpfPu - coef->uwVdcCvbPu) * coef->uwKAnglePu) >> 7; // Q15
    }
    else
    {
        swDelta = ((SLONG)(coef->uwVdcLagAngelPu - coef->uwVdcCvbPu) * coef->uwKAnglePu) >> 7; // Q15
    }

    /*Id limit*/
    if (out->swIdRefPu > coef->swIdRefMaxPu)
    {
        out->swIdRefPu = coef->swIdRefMaxPu;
    }
    else if (out->swIdRefPu < coef->swIdRefMinPu)
    {
        out->swIdRefPu = coef->swIdRefMinPu;
    }
    else
    {
        out->swIdRefPu = out->swIdRefPu;
    }

    /*Iq limit*/
    if (in->swSpdPu > 0)
    {
        out->swIqLimPu = -out->swIqLimPu;
        if (in->swIqRefPu < out->swIqLimPu)
        {
            out->swIqRefPu = out->swIqLimPu;
        }
        else
        {
            out->swIqRefPu = in->swIqRefPu;
        }
        out->uwAngelPu = in->uwAngelPu - swDelta + cof_sl720DegreePu;
    }
    else
    {
        out->swIqLimPu = out->swIqLimPu;
        if (in->swIqRefPu > out->swIqLimPu)
        {
            out->swIqRefPu = out->swIqLimPu;
        }
        else
        {
            out->swIqRefPu = in->swIqRefPu;
        }
        out->uwAngelPu = in->uwAngelPu + swDelta + cof_sl720DegreePu;
    }
}
/************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************/
#ifdef _BRAKE_C_
#undef _BRAKE_C_
#endif
/*************************************************************************
 End of this File (EOF):
 !!!!!!Do not put anything after this part!!!!!!!!!!!
*************************************************************************/