motor_ctl-4/User project/2.MotorDrive/Source/adc.c

/************************************************************************
 Project:             Welling Motor Control Paltform
 Filename:            adc.c
 Partner Filename:    adc.h
 Description:         Get the adc conversion results
 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 _ADCDRV_C_
#define _ADCDRV_C_
#endif

/************************************************************************
 Included File:
*************************************************************************/
#include "syspar.h"
#include "user.h"
#include "can.h"
#include "Temp.h"
/************************************************************************
 Constant Table:
*************************************************************************/

/************************************************************************
 Exported Functions:
*************************************************************************/
/***************************************************************
 Function: adc_voCalibration;
 Description: Get phase A and B current zero point, other A/D sample value
 Call by: main() before InitADC;
 Input Variables: N/A
 Output/Return Variables: ADCTESTOUT
 Subroutine Call: N/A
 Reference: N/A
****************************************************************/
void adc_voCalibration(ADC_COF *cof, ADC_DOWN_OUT *out1, ADC_UP_OUT *out2)
{

    if (out1->blADCCalibFlg == FALSE || out2->blADCCalibFlg == FALSE)
    {
        if (!hw_blChrgOvrFlg)
        {
            hw_voCharge();
        }
        else
        {
            if(cp_stFlg.CurrentSampleModelSelect == SINGLERESISITANCE)
            {
                if (out1->uwADCCalibCt < (1 << ADC_CALIB_INDEX))
                {
                    out1->ulIdcRegSum += adc_uwADDMAPhase1 + adc_uwADDMAPhase2;
                    out1->uwADCCalibCt++;
                }
                else if (out2->uwADCCalibCt < (1 << ADC_CALIB_INDEX))
                {
                    out2->uwADCCalibCt++;
                }
                else
                {
                    hw_voPWMInit();
                    cof->uwIdcOffset = out1->ulIdcRegSum >> (ADC_CALIB_INDEX + 1);
                    out1->ulIdcRegSum = 0;
                    out1->uwADCCalibCt = 0;
                    out1->blADCCalibFlg = TRUE;
                    out2->uwADCCalibCt = 0;
                    out2->blADCCalibFlg = TRUE;
                }
            }
            else if(cp_stFlg.CurrentSampleModelSelect == RDSON)
            {
                if (out1->uwADCCalibCt < (1 << ADC_CALIB_INDEX))
                {
                    out1->ulIaRegSum += adc_uwRdsonUReg;
                    out1->ulIbRegSum += adc_uwRdsonVReg;
                    out1->ulIcRegSum += adc_uwRdsonWReg;
                    out1->uwADCCalibCt++;
                }
                else
                {
                    hw_voPWMInit();
                    
                    cof->uwIaOffset = out1->ulIaRegSum >> (ADC_CALIB_INDEX);
                    cof->uwIbOffset = out1->ulIbRegSum >> (ADC_CALIB_INDEX);
                    cof->uwIcOffset = out1->ulIcRegSum >> (ADC_CALIB_INDEX);

                    out1->ulIaRegSum = 0;
                    out1->ulIbRegSum = 0;
                    out1->ulIcRegSum = 0;
                    out1->uwADCCalibCt = 0;
                    out1->blADCCalibFlg = TRUE;
                    out2->uwADCCalibCt = 0;
                    out2->blADCCalibFlg = TRUE;
                }
            }
            else if(cp_stFlg.CurrentSampleModelSelect == COMBINATION)
            {
//                pwm_stGenOut.uwRDSONTrig = 108;  
                pwm_stGenOut.uwRDSONTrig = 129; 
                pwm_stGenOut.uwSigRTrig = HW_HHHPWM_PERIOD;
                pwm_stGenOut.blSampleCalibFlag = TRUE;
                if (out1->uwADCCalibCt < (1 << ADC_CALIB_INDEX))
                {
                    out1->ulIdcRegSum += adc_uwADDMAPhase1;
                    out1->ulIaRegSum += adc_uwRdsonUReg;
                    out1->ulIbRegSum += adc_uwRdsonVReg;
                    out1->ulIcRegSum += adc_uwRdsonWReg;
                    out1->uwADCCalibCt++;
                }
                else
                {
                    hw_voPWMInit(); // mos up charge and adc calib over; pwm off
                    cof->uwIaOffset = out1->ulIaRegSum >> (ADC_CALIB_INDEX);
                    cof->uwIbOffset = out1->ulIbRegSum >> (ADC_CALIB_INDEX);
                    cof->uwIcOffset = out1->ulIcRegSum >> (ADC_CALIB_INDEX);

                    out1->ulIaRegSum = 0;
                    out1->ulIbRegSum = 0;
                    out1->ulIcRegSum = 0;

                    pwm_stGenOut.blSampleCalibFlag = FALSE;
                    cof->uwIdcOffset = out1->ulIdcRegSum >> (ADC_CALIB_INDEX);
                    out1->ulIdcRegSum = 0;

                    out1->uwADCCalibCt = 0;
                    out1->blADCCalibFlg = TRUE;
                    out2->uwADCCalibCt = 0;
                    out2->blADCCalibFlg = TRUE;
                }
            }
            else
            {}
        }
    }
}
/***************************************************************
 Function: adc_voSample;
 Description: Get three-phase current value after zero point and gain process
 Call by: functions in TBC;
 Input Variables: ADCIABFIXCOF
 Output/Return Variables: ADCTESTOUT
 Subroutine Call:
 Reference: N/A
****************************************************************/
void adc_voSampleDown(ADC_COF *cof, ADC_DOWN_OUT *out)
{
    UWORD uwIpeakPu;

    if(cp_stFlg.CurrentSampleModelSelect == SINGLERESISITANCE)
    {
        SWORD tmp_swIphase1, tmp_swIphase2, tmp_swIphase3;

        /* Register value */
        out->uwFirstCurREG = adc_uwADDMAPhase1;  // Q12
        out->uwSecondCurREG = adc_uwADDMAPhase2; // Q12

        tmp_swIphase1 = (SWORD)out->uwFirstCurREG - cof->uwIdcOffset;
        tmp_swIphase1 = ((SLONG)tmp_swIphase1 * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10
        tmp_swIphase2 = (SWORD)cof->uwIdcOffset - out->uwSecondCurREG;
        tmp_swIphase2 = ((SLONG)tmp_swIphase2 * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10
        tmp_swIphase3 = (SWORD)out->uwSecondCurREG - out->uwFirstCurREG;
        tmp_swIphase3 = ((SLONG)tmp_swIphase3 * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10

        out->uwADCSector = pwm_stGenOut.uwNewSectorNum;
        switch (pwm_stGenOut.uwNewSectorNum)
        {
            case 1:
                out->swIbPu = tmp_swIphase1; // v
                out->swIcPu = tmp_swIphase2; //-w
                out->swIaPu = tmp_swIphase3; // u
                break;
            case 2:
                out->swIaPu = tmp_swIphase1; // u
                out->swIbPu = tmp_swIphase2; //-v
                out->swIcPu = tmp_swIphase3;
                break;
            case 3:
                out->swIaPu = tmp_swIphase1; // u
                out->swIcPu = tmp_swIphase2; //-w
                out->swIbPu = tmp_swIphase3;
                break;
            case 4:
                out->swIcPu = tmp_swIphase1; // w
                out->swIaPu = tmp_swIphase2; //-u
                out->swIbPu = tmp_swIphase3;
                break;
            case 5:
                out->swIbPu = tmp_swIphase1; // v
                out->swIaPu = tmp_swIphase2; //-u
                out->swIcPu = tmp_swIphase3;
                break;
            case 6:
                out->swIcPu = tmp_swIphase1; // w
                out->swIbPu = tmp_swIphase2; //-v
                out->swIaPu = tmp_swIphase3;
                break;
            default:
                out->swIaPu = 0;
                out->swIbPu = 0;
                out->swIcPu = 0;
                break;
        }
    }
    else if(cp_stFlg.CurrentSampleModelSelect == RDSON)
    {
       SWORD tmp_swIphase1, tmp_swIphase2, tmp_swIphase3;
        out->uwIaReg = adc_uwRdsonUReg;
        out->uwIbReg = adc_uwRdsonVReg;
        out->uwIcReg = adc_uwRdsonWReg;

        tmp_swIphase1 = -(((SWORD)out->uwIaReg - cof->uwIaOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10
        tmp_swIphase2 = -(((SWORD)out->uwIbReg - cof->uwIbOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10
        tmp_swIphase3 = -(((SWORD)out->uwIcReg - cof->uwIcOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10

        switch (pwm_stGenOut.uwSampleArea)
        {
        case IgnoreNone:
            out->swIaPu = tmp_swIphase1;
            out->swIbPu = tmp_swIphase2;
            out->swIcPu = tmp_swIphase3;
            break;
        case IgnoreA:
            out->swIaPu = -tmp_swIphase2 - tmp_swIphase3;
            out->swIbPu = tmp_swIphase2;
            out->swIcPu = tmp_swIphase3;
            break;
        case IgnoreB:
            out->swIaPu = tmp_swIphase1;
            out->swIbPu = -tmp_swIphase1 - tmp_swIphase3;
            out->swIcPu = tmp_swIphase3;
            break;
        case IgnoreC:
            out->swIaPu = tmp_swIphase1;
            out->swIbPu = tmp_swIphase2;
            out->swIcPu = tmp_swIphase3;
            break;
        case IgnoreAB:
            out->swIaPu = -tmp_swIphase3 >> 1;
            out->swIbPu = -tmp_swIphase3 >> 1;
            out->swIcPu = tmp_swIphase3;
            break;
        case IgnoreBC:
            out->swIaPu = tmp_swIphase1;
            out->swIbPu = -tmp_swIphase1 >> 1;
            out->swIcPu = -tmp_swIphase1 >> 1;
            break;
        case IgnoreAC:
            out->swIaPu = -tmp_swIphase2 >> 1;
            out->swIbPu = tmp_swIphase2;
            out->swIcPu = -tmp_swIphase2 >> 1;
            break;
        default:
            break;
        } 
    }
    else if(cp_stFlg.CurrentSampleModelSelect == COMBINATION)
    {
        out->uwIaReg = adc_uwRdsonUReg;
        out->uwIbReg = adc_uwRdsonVReg;
        out->uwIcReg = adc_uwRdsonWReg;

        out->slSampIaPu = -((SLONG)((SLONG)out->uwIaReg - (SLONG)cof->uwIaOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10
        out->slSampIbPu = -((SLONG)((SLONG)out->uwIbReg - (SLONG)cof->uwIbOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10
        out->slSampIcPu = -out->slSampIaPu - out->slSampIbPu;   //-((SLONG)((SLONG)out->uwIcReg - (SLONG)cof->uwIcOffset) * cof->uwCurReg2Pu >> 10); // Q14=Q24-Q10
//      cof->uwCalibcoef = 1500;
        out->swIaPu = ((SLONG)out->slSampIaPu * cof->uwCalibcoef) >> 10;
        out->swIbPu = ((SLONG)out->slSampIbPu * cof->uwCalibcoef) >> 10;
        out->swIcPu = ((SLONG)out->slSampIcPu * cof->uwCalibcoef) >> 10;
    }
    else
    {}

    /* Current absolute value & max value */
    if ((out->swIaPu) >= 0)
    {
        out->uwIaAbsPu = (UWORD)out->swIaPu;
    }
    else
    {
        out->uwIaAbsPu = (UWORD)(-(out->swIaPu));
    }
    if ((out->swIbPu) >= 0)
    {
        out->uwIbAbsPu = (UWORD)out->swIbPu;
    }
    else
    {
        out->uwIbAbsPu = (UWORD)(-(out->swIbPu));
    }
    if ((out->swIcPu) >= 0)
    {
        out->uwIcAbsPu = (UWORD)out->swIcPu;
    }
    else
    {
        out->uwIcAbsPu = (UWORD)(-(out->swIcPu));
    }

    uwIpeakPu = out->uwIaAbsPu > out->uwIbAbsPu ? out->uwIaAbsPu : out->uwIbAbsPu;
    uwIpeakPu = out->uwIcAbsPu > uwIpeakPu ? out->uwIcAbsPu : uwIpeakPu;
    out->uwIpeakPu = uwIpeakPu;
}

void adc_voSampleUp(ADC_COF *cof, ADC_UP_OUT *out)
{
    /* Register value */
    out->uwVdcReg = hw_uwADC0[0];
    out->uwU6VReg = hw_uwADC0[1];
    out->uwU5VReg = hw_uwADC0[2];
    out->PCBTempReg = hw_uwADC0[3];
    out->TorqTempReg = hw_uwADC0[4];
    out->uwU12VReg = hw_uwADC0[5];
    out->uwThrottleReg = hw_uwADC0[6];

    out->uwVdcPu = (SLONG)out->uwVdcReg * cof->uwVdcReg2Pu >> 10; // Q14=Q24-Q10
    /* Vdc LPF */
    out->uwVdcLpfPu = ((out->uwVdcPu - out->uwVdcLpfPu) >> 1) + out->uwVdcLpfPu;

    out->uwU6VPu = (SLONG)out->uwU6VReg * cof->uwU6VReg2Pu >> 10;    // Q14=Q24-Q10;
    out->uwU5VPu = (SLONG)out->uwU5VReg * cof->uwU5VReg2Pu >> 10;    // Q14=Q24-Q10;
    out->uwU12VPu = (SLONG)out->uwU12VReg * cof->uwU12VReg2Pu >> 10; // Q14=Q24-Q10;

    out->MotorTempR = out->MotorTempReg * cof->swMotorTempKcof >> 10; // Q14=Q24-Q10;

    ////////////////// Single Resitance Current Sample//////////////////////////////////////////////////////
    if (pwm_stGenOut.blSampleCalibFlag == TRUE)
    {
        switch (pwm_stGenOut.uwSingelRSampleArea)
        {
        case 0:
            out->swCalibIaPu = 0;
            out->swCalibIbPu = 0;
            out->swCalibIcPu = 0;
            break;
        case SampleA:
            out->swCalibIaPu = -((SLONG)((SWORD)adc_uwADDMAPhase1 - cof->uwIdcOffset) * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10
            break;
        case SampleB:
            out->swCalibIbPu = -((SLONG)((SWORD)adc_uwADDMAPhase1 - cof->uwIdcOffset) * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10
            break;
        case SampleC:
            out->swCalibIcPu = -((SLONG)((SWORD)adc_uwADDMAPhase1 - cof->uwIdcOffset) * cof->uwCurIdcReg2Pu) >> 10; // Q14=Q24-Q10
            break;
        default:
            break;
        }
    }

    ////////////////// PCB TEMP//////////////////////////////////////////////////////
    out->PCBTempR = (ULONG)4096 * PCB_TEMP_SAMPLER / out->PCBTempReg - PCB_TEMP_SAMPLER; // Q14=Q24-Q10;
    PcbTempCal(out->PCBTempR);
    out->PCBTemp = tmp_PcbTemp;
}

SWORD swSingleReg,GainTemp2;
SLONG slRdsonReg;
LPF_OUT adc_pvt_stRdsonCoefLpf;
UWORD uwCalGainflg;
ULONG GainTemp1;
void adc_voSRCalibration(ADC_COF *cof, ADC_UP_OUT *up_out, ADC_DOWN_OUT *down_out)
{ 

    if (pwm_stGenOut.blSampleCalibFlag == TRUE)
    {             

        switch (pwm_stGenOut.uwSingelRSampleArea)
        {
        case 0:
            break;
        case SampleA:
            if(swSingleReg > up_out->swCalibIaPu)
            {
              swSingleReg = up_out->swCalibIaPu;
            }
            if(slRdsonReg > down_out->slSampIaPu)
            {
              slRdsonReg = down_out->slSampIaPu;
            }
            
            break;
        case SampleB:
            if(swSingleReg > up_out->swCalibIbPu)
            {
              swSingleReg = up_out->swCalibIbPu;
            }
            if(slRdsonReg > down_out->slSampIbPu)
            {
              slRdsonReg = down_out->slSampIbPu;
            }
            break;
        case SampleC:
            if(swSingleReg > up_out->swCalibIcPu)
            {
              swSingleReg = up_out->swCalibIcPu;
            }    
            if(slRdsonReg > down_out->slSampIcPu)
            {
              slRdsonReg = down_out->slSampIcPu;
            }
            break;
        default:
            break;
        }
      
        uwCalGainflg = 1;
           
    }
    else
    {
         if(scm_uwSpdFbkLpfAbsPu <= 2500)
         {

            GainTemp1 = 1300;
         }
         else
         {
            if(uwCalGainflg ==1)
            {
                GainTemp1 = (SLONG)((SLONG)swSingleReg << 10) / (SLONG)slRdsonReg;               
                if(GainTemp1 > cof->uwCalibcoefMax)
                {
                    GainTemp1 = cof->uwCalibcoefMax;
                }
                else if(GainTemp1 < cof->uwCalibcoefMin)
                {
                    GainTemp1 = cof->uwCalibcoefMin;
                }
                else
                {
                }
                uwCalGainflg = 0;
               
               }
            else
            {
                swSingleReg = 0;
                slRdsonReg = 0;
            }
         }
                  
        mth_voLPFilter(GainTemp1, &adc_pvt_stRdsonCoefLpf);  
        cof->uwCalibcoef = adc_pvt_stRdsonCoefLpf.slY.sw.hi; 
    }
      ////////////////////////////////////////////////////////////////////////////////////
       
}

/***************************************************************
 Function: adc_voSampleCoef;
 Description: Get other A/D sample value
 Call by: functions in Mainloop;
 Input Variables: ADCIABFIXCOF
 Output/Return Variables: ADCTESTOUT
 Subroutine Call:
 Reference: N/A
****************************************************************/
void adc_voSampleCoef(ADC_COF *cof)
{
    cof->uwCurReg2Pu = ((UQWORD)ADC_IPHASE_CUR_MAX_AP << 24) / (1 << (ADC_RESOLUTION_BIT - 1)) / IBASE; // Q24
    cof->uwCurIdcReg2Pu = ((UQWORD)ADC_IDC_CUR_MAX_AP << 24) / (1 << (ADC_RESOLUTION_BIT)) / IBASE;     // Q24
    cof->uwVdcReg2Pu = ((UQWORD)ADC_VDC_MAX_VT << 24) / (1 << ADC_RESOLUTION_BIT) / VBASE;              // Q24
    cof->uwUabcReg2Pu = ((UQWORD)ADC_UABC_MAX_VT << 24) / (1 << (ADC_RESOLUTION_BIT)) / VBASE;          // Q24
    cof->uwU6VReg2Pu = ((UQWORD)ADC_LIGHT_MAX_VT << 24) / (1 << (ADC_RESOLUTION_BIT)) / VBASE;          // Q24;
    cof->uwU5VReg2Pu = ((UQWORD)ADC_SPDSENSOR_MAX_VT << 24) / (1 << (ADC_RESOLUTION_BIT)) / VBASE;      // Q24;
    cof->uwU12VReg2Pu = ((UQWORD)ADC_DISPLAY_MAX_VT << 24) / (1 << (ADC_RESOLUTION_BIT)) / VBASE;       // Q24;
    cof->uwCalibcoef = 1024;
    cof->uwCalibcoefMax = 2048;
    cof->uwCalibcoefMin = 200;
    cof->uwCalibCoefK = 160; // q10
    mth_voLPFilterCoef(1000000 / 30, FTBC_HZ, &adc_pvt_stRdsonCoefLpf.uwKx); //100Hz
}
/***************************************************************
 Function: adc_voSampleInit;
 Description: ADC sample initialization
 Call by: mn_voSoftwareInit;
 Input Variables: N/A
 Output/Return Variables: N/A
 Subroutine Call:
 Reference: N/A
****************************************************************/
void adc_voSampleInit(void)
{
    adc_stDownOut.swIaPu = 0;
    adc_stDownOut.swIbPu = 0;
    adc_stDownOut.swIcPu = 0;
    adc_stDownOut.uwIaAbsPu = 0;
    adc_stDownOut.uwIbAbsPu = 0;
    adc_stDownOut.uwIcAbsPu = 0;
    adc_stDownOut.uwIpeakPu = 0;
    adc_stDownOut.uwIaReg = 0;
    adc_stDownOut.uwIbReg = 0;
    adc_stDownOut.uwIcReg = 0;
    adc_stDownOut.uwFirstCurREG = 0;
    adc_stDownOut.uwSecondCurREG = 0;
    adc_stDownOut.uwADCSector = 0;
    adc_stDownOut.uwIaAvgPu = 0;
    adc_stDownOut.uwIbAvgPu = 0;
    adc_stDownOut.uwIcAvgPu = 0;

    adc_stUpOut.uwVdcPu = 0;
    adc_stUpOut.uwVdcLpfPu = 0;
    adc_stUpOut.uwU6VPu = 0;
    adc_stUpOut.uwU5VPu = 0;
    adc_stUpOut.uwU12VPu = 0;
    adc_stUpOut.uwTrottlePu = 0;
    adc_stUpOut.PCBTemp = 0;
    adc_stUpOut.MotorTemp = 0;
    adc_stUpOut.uwVdcReg = 0;
    adc_stUpOut.uwU6VReg = 0;
    adc_stUpOut.uwU5VReg = 0;
    adc_stUpOut.uwU12VReg = 0;
    adc_stUpOut.uwThrottleReg = 0;
    adc_stUpOut.PCBTempReg = 0;
    adc_stUpOut.MotorTempReg = 0;
    adc_stUpOut.swCalibIaPu = 0;
    adc_stUpOut.swCalibIbPu = 0;
    adc_stUpOut.swCalibIcPu = 0;

    adc_stDownOut.ulUaRegSum = 0;
    adc_stDownOut.ulUbRegSum = 0;
    adc_stDownOut.ulUcRegSum = 0;
    adc_stDownOut.ulIdcRegSum = 0;
    adc_stDownOut.ulIaRegSum = 0;
    adc_stDownOut.ulIbRegSum = 0;
    adc_stDownOut.ulIcRegSum = 0;
    adc_stDownOut.uwADCCalibCt = 0;
    adc_stDownOut.blADCCalibFlg = FALSE;

    adc_stUpOut.uwADCCalibCt = 0;
    adc_stUpOut.blADCCalibFlg = FALSE;
    adc_stUpOut.swIPMTempCe = 0;
}

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

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