/************************************************************************
 Project:             Welling FAN Motor Developing platform
 Filename:            cmdgen.c
 Partner Filename:    cmdgen.h
 Description:         Partner file of cmdgen.h
 Complier:            IAR Embedded Workbench for ARM 7.80, IAR Systems.
 CPU TYPE :           ST32F0xx
*************************************************************************
 Copyright (c) 2019 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************
*************************************************************************
Revising History (ECL of this file):
20191225, by cyf, create this file;
************************************************************************/
#define FALSE                          (0U)
#define TRUE                           (1U)

#define RESET                          (0U)
#define SET                           (1U)

#define abs(A)             (((A) < 0) ? - (A) : (A)) 
/************************************************************************
 Beginning of File, do not put anything above here except notes
 Compiler Directives:
*************************************************************************/
#ifndef _CMDGENNEW_C_
#define _CMDGENNEW_C_
#endif
/************************************************************************
 Included File
*************************************************************************/
#include "cmdgennew.h"
#include "glbcof.h"
/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: cmd_voCmdInit;
 Description: Command initialization
 Call by: functions in main loop;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void cmd_voCmdInit(void)
{
    cmd_stCmdOut.swNewCmdDir = 1;
    cmd_stCmdOut.swNewSpdRefPu = 0;
    cmd_stCmdOut.swOldCmdDir = 1;
    cmd_stCmdOut.swOldSpdRefPu = 0;
    cmd_stCmdOut.blConsVolBrakeFlg = RESET;
    cmd_stCmdOut.swMotorDir = 1;
    cmd_stCmdOut.swIntRefPu = 0;
    cmd_stCmdOut.slIntRefPu = 0;
}
/***************************************************************
 Function: cmd_voCmdCoef;
 Description: Coefficient calculation for Command
 Call by: functions in Mainloop;
 Input Variables: CMD_CMDCOFIN
 Output/Return Variables: CMD_CMDCOF
 Subroutine Call:
 Reference: N/A
****************************************************************/
void cmd_voCmdCoef(CMD_CMDCOFIN *in, CMD_CMDCOF *coef)
{
    coef->ulAccelPu = in->ulAccelPu;                 // Q29
    coef->ulDecelPu = in->ulDecelPu;                 // Q29
    coef->swBrakeSpdDeltaPu = in->swBrakeSpdDeltaPu; // Q15
}
/***************************************************************
 Function: cmd_voCmdOut;
 Description: Calculation for Command
 Call by: functions in TBS;
 Input Variables: CMD_CMDIN
 Output/Return Variables: CMD_CMDOUT
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void cmd_voCmdOut(CMD_CMDIN *in, CMD_CMDCOF *coef, CMD_CMDOUT *out)
{
    SWORD newCMD;
    if (in->swSpdCmdRpm > 0)
    {
        out->swNewCmdDir = 1;
    }
    else if (in->swSpdCmdRpm < 0)
    {
        out->swNewCmdDir = -1;
    }
    else
    {
        out->swNewCmdDir = out->swOldCmdDir;
    }

    if (in->swSpdCmdRpm > 6000)
    {
        newCMD = 6000;
    }
    else if (in->swSpdCmdRpm < -6000)
    {
        newCMD = -6000;
    }
    else
    {
        newCMD = in->swSpdCmdRpm;
    }

    out->swNewSpdRefPu = ((SLONG)newCMD << 15) / cof_uwVbRpm; // Q15
    if (out->swNewSpdRefPu != out->swOldSpdRefPu)
    {
        if (out->swOldSpdRefPu != 0)
        {
            /*----if new cmd direction is same with the old-----*/
            if (out->swNewCmdDir == out->swOldCmdDir)
            {
                /*if new command speed lower than the old*/
                if ((abs(out->swOldSpdRefPu) - abs(out->swNewSpdRefPu)) > coef->swBrakeSpdDeltaPu)
                {
                    out->blConsVolBrakeFlg = SET;
                    /*actual speed lower than the new, disable CVB*/
                    if ((in->swSpdNowPu * out->swNewCmdDir) < abs(out->swNewSpdRefPu))
                    {
                        out->blConsVolBrakeFlg = RESET;
                    }
                }
                /*if new command speed higer than the old*/
                else
                {
                    /*if CVB enabled,disable CVB and initial some parameters*/
                    if (out->blConsVolBrakeFlg == SET)
                    {
                        out->blConsVolBrakeFlg = RESET;
                    }
                }
            }
            else
            {}
        }
    }
    /*if the new command speed the same with the old*/
    else
    {
        /*if CVB enabled, disable CVB and initial some parameters*/
        if (out->blConsVolBrakeFlg == SET)
        {
            out->blConsVolBrakeFlg = RESET;
        }
    }
    /*-if CVB disabled, load the new command to the old-----*/
    if (out->blConsVolBrakeFlg == RESET)
    {
        out->swOldSpdRefPu = out->swNewSpdRefPu;
        out->swOldCmdDir = out->swNewCmdDir;
    }
    SWORD swIntRefPu;     // Q15
    SLONG slIntRefPu;     // Q29
    SWORD swUserSpdRefPu; // Q15
    SLONG slUserSpdRefPu; // Q29
    ULONG ulUserSpdRefAbsPu;
    ULONG ulIntRefAbsPu;
    ULONG ulAccDeltaPu, ulDecDeltaPu;
    /*--------------------------------------------------------------------------
                            |    COF->slAccDeltaPu      |  OUTPUT->swIntRefPu
    INPUT->swUSART_RefCmdPu |    COF->slDecDeltaPu      |  OUTPUT->swUSART_RefCmdZ1Pu
                            |                           |  OUTPUT->swMotorDir
    --------------------------------------------------------------------------*/
    swIntRefPu = out->swIntRefPu; // Intermediant Speed
    slIntRefPu = out->slIntRefPu;
    swUserSpdRefPu = out->swNewSpdRefPu;          // Target Speed
    slUserSpdRefPu = (SLONG)swUserSpdRefPu << 14; // Q29=Q15+Q14
    ulUserSpdRefAbsPu = abs(slUserSpdRefPu);
    ulIntRefAbsPu = abs(slIntRefPu);
    /* Command Referance Speed Run Direction*/
    if (swIntRefPu < 0)
    {
        out->swMotorDir = -1;
    }
    else if (swIntRefPu > 0)
    {
        out->swMotorDir = 1;
    }
    else
    {
        out->swMotorDir = out->swNewCmdDir;
    }
    ulAccDeltaPu = coef->ulAccelPu; // Q29
    ulDecDeltaPu = coef->ulDecelPu; // Q29
    /*-------------------------------------------------------------------------
                            Usart Command Changing
    --------------------------------------------------------------------------*/
    if (out->swMotorDir != out->swNewCmdDir)
    {
        slIntRefPu += (ulDecDeltaPu * out->swNewCmdDir);
    }
    else
    {
        if (ulIntRefAbsPu < ulUserSpdRefAbsPu)
        {
            slIntRefPu += (ulAccDeltaPu * out->swNewCmdDir); // Q29
            if (abs(slIntRefPu) >= ulUserSpdRefAbsPu)
            {
                slIntRefPu = slUserSpdRefPu;
            }
        }
        else if (ulIntRefAbsPu > ulUserSpdRefAbsPu)
        {
            slIntRefPu -= (ulDecDeltaPu * out->swNewCmdDir); // Q29
            if (ulUserSpdRefAbsPu > 0)
            {
                if (abs(slIntRefPu) <= ulUserSpdRefAbsPu)
                {
                    slIntRefPu = slUserSpdRefPu;
                }
            }
            else
            {
                if ((slIntRefPu * out->swNewCmdDir) <= 0)
                {
                    slIntRefPu = 0;
                }
            }
        }
        else
        {
            slIntRefPu = slUserSpdRefPu;
        }
    }

    out->slIntRefPu = slIntRefPu;              // Q29
    out->swIntRefPu = (out->slIntRefPu) >> 14; // Q29-Q14=Q15
}
/************************************************************************
  Local Functions:
*************************************************************************/

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