/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            crdnt.c
 Partner Filename:    crdnt.h
 Description:         Coordinate transformation
 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 _CRDNT_C_
#define _CRDNT_C_
#endif

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

/************************************************************************
 Constant Table:
*************************************************************************/

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: crd_voClark;
 Description: clark transformation
 Call by: functions in TBC;
 Input Variables: CRD_CLARK_IN
 Output/Return Variables: CRD_CLARK_OUT(Q format=CRD_CLARK_IN)
 Subroutine Call: N/A;
 Reference: N/A
****************************************************************/
void crd_voClark(const CRD_CLARK_IN *in, CRD_CLARK_OUT *out)
{
    SLONG slClark; //(Q15)

    /*--------------------------------------
    Ialfa = 2/3 * ia - 1/3 * ib - 1/3 * ic
    ---------------------------------------*/
    slClark = ((SLONG)in->swAPu * 2 - in->swBPu - in->swCPu) * 10923 >> 15; // 10923    1/3 (Q15)
    if (slClark > 32767)
    {
        out->swAlphaPu = 32767;
    }
    else if (slClark < -32768)
    {
        out->swAlphaPu = -32768;
    }
    else
    {
        out->swAlphaPu = (SWORD)slClark;
    }
    /*--------------------------------------
    Ibeta = 1 / sqrt(3) * (ib - ic)
    --------------------------------------*/
    slClark = ((SLONG)in->swBPu - in->swCPu) * 18919 >> 15; // 18919    1/sqrt(3) (Q15)
    if (slClark > 32767)
    {
        out->swBetaPu = 32767;
    }
    else if (slClark < -32768)
    {
        out->swBetaPu = -32768;
    }
    else
    {
        out->swBetaPu = (SWORD)slClark;
    }
}

/***************************************************************
 Function: crd_voPark;
 Description: Park transformation
 Call by: functions in TBC;
 Input Variables: CRD_PARK_IN
 Output/Return Variables: CRD_PARK_OUT
 Subroutine Call: mth_voSinCos;
 Reference: N/A
****************************************************************/
void crd_voPark(const CRD_PARK_IN *in, CRD_PARK_OUT *out)
{
    SLONG  slPark; //(Q15)
    SINCOS stPark_SINCOS;

    mth_voSinCos(in->uwThetaPu, &stPark_SINCOS); // calculate sin and cos

    /*--------------------------------------
    id = ialfa * cos + ibeta * sin
    ---------------------------------------*/
    slPark = ((SLONG)in->swAlphaPu * stPark_SINCOS.swCosPu >> 15) + ((SLONG)in->swBetaPu * stPark_SINCOS.swSinPu >> 15);
    if (slPark > 32767)
    {
        out->swDPu = 32767;
    }
    else if (slPark < -32768)
    {
        out->swDPu = -32768;
    }
    else
    {
        out->swDPu = (SWORD)slPark;
    }

    /*--------------------------------------
    iq = -ialfa * sin + ibeta * cos
    ---------------------------------------*/
    slPark = (-(SLONG)in->swAlphaPu * stPark_SINCOS.swSinPu >> 15) + ((SLONG)in->swBetaPu * stPark_SINCOS.swCosPu >> 15);
    if (slPark > 32767)
    {
        out->swQPu = 32767;
    }
    else if (slPark < -32768)
    {
        out->swQPu = -32768;
    }
    else
    {
        out->swQPu = (SWORD)slPark;
    }
}

/***************************************************************
 Function: crd_voIpark;
 Description: inverse park transformation
 Call by: functions in TBC;
 Input Variables: CRD_IPARK_IN
 Output/Return Variables: CRD_IPARK_OUT
 Subroutine Call: mth_vo_SinCos;
 Reference: N/A
****************************************************************/
void crd_voIPark(const CRD_IPARK_IN *in, CRD_IPARK_OUT *out)
{
    SLONG  slIPark; //(Q15)
    SINCOS stIPark_SINCOS;

    mth_voSinCos(in->uwThetaPu, &stIPark_SINCOS); // calculate sin and cos

    /*--------------------------------------
    ialfa = id * cos - iq * sin
    --------------------------------------*/
    slIPark = ((SLONG)in->swDPu * stIPark_SINCOS.swCosPu >> 15) - ((SLONG)in->swQPu * stIPark_SINCOS.swSinPu >> 15);
    if (slIPark > 32767)
    {
        out->swAlphaPu = 32767;
    }
    else if (slIPark < -32768)
    {
        out->swAlphaPu = -32768;
    }
    else
    {
        out->swAlphaPu = (SWORD)slIPark;
    }

    /*--------------------------------------
    ibeta = id * sin + iq * cos
    --------------------------------------*/
    slIPark = ((SLONG)in->swDPu * stIPark_SINCOS.swSinPu >> 15) + ((SLONG)in->swQPu * stIPark_SINCOS.swCosPu >> 15);
    if (slIPark > 32767)
    {
        out->swBetaPu = 32767;
    }
    else if (slIPark < -32768)
    {
        out->swBetaPu = -32768;
    }
    else
    {
        out->swBetaPu = (SWORD)slIPark;
    }
}

/***************************************************************
 Function: crd_voIClark;
 Description: inverse clark transformation
 Call by: functions in TBC;
 Input Variables: CRD_ICLARK_IN
 Output/Return Variables: CRD_ICLARK_OUT
 Subroutine Call:
 Reference: N/A
****************************************************************/
void crd_voIClark(const CRD_ICLARK_IN *in, CRD_ICLARK_OUT *out)
{
    SLONG slIClark;

    /*--------------------------------------
    ia = ialfa
    ---------------------------------------*/
    out->swIClarkaPu = in->swIClarkAlfaPu;

    /*--------------------------------------
    ib = -1/2 * ialfa + sqrt(3)/2 * ibeta
    ---------------------------------------*/
    slIClark = (((SLONG)in->swIClarkBetaPu * 28378 >> 14) - in->swIClarkAlfaPu) >> 1; // Tmp (Q15),28378=sqrt(3) Q(14)
    if (slIClark > 32767)
    {
        out->swIClarkbPu = 32767;
    }
    else if (slIClark < -32768)
    {
        out->swIClarkbPu = -32768;
    }
    else
    {
        out->swIClarkbPu = (SWORD)slIClark;
    }

    /*--------------------------------------
    ic = -1/2 * ialfa - sqrt(3)/2 * ibeta
    ---------------------------------------*/
    slIClark = (-((SLONG)in->swIClarkBetaPu * 28378 >> 14) - in->swIClarkAlfaPu) >> 1; // Tmp (Q15),28378=sqrt(3) Q(14)
    if (slIClark > 32767)
    {
        out->swIClarkcPu = 32767;
    }
    else if (slIClark < -32768)
    {
        out->swIClarkcPu = -32768;
    }
    else
    {
        out->swIClarkcPu = (SWORD)slIClark;
    }
}
/************************************************************************
 Local Functions: N/A
*************************************************************************/

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