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 "Temp.h"
#include "api.h"
#include "api_rt.h"

#ifdef RUN_ARCH_SIM
#include "test_user.h"
#endif
/************************************************************************
 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
****************************************************************/
#ifdef RUN_ARCH_SIM

#else
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 == COMBINATION)
            {
                ULONG samplingTick[2];
                samplingTick[0]=HW_HHHPWM_PERIOD;
                samplingTick[1]=129;
                iPwm_SyncMultiSamplingCountUp(0, &samplingTick[0], 2);
                pwm_stGenOut.blSampleCalibFlag = TRUE;
                if (out1->uwADCCalibCt < (1 << ADC_CALIB_INDEX))
                {
                    out1->ulIdcRegSum += iAdc_GetResultPointer(2)[HW_ADC_IDC_CH];
                    out1->ulIaRegSum += iAdc_GetResultPointer(1)[HW_ADC_IA_CH];
                    out1->ulIbRegSum += iAdc_GetResultPointer(1)[HW_ADC_IB_CH];
                    out1->ulIcRegSum += iAdc_GetResultPointer(1)[HW_ADC_IC_CH];
                    out1->uwADCCalibCt++;
                }
                else
                {
                    hw_voPWMInit(); // mos up charge and adc calib over; pwm off
                    cof->uwIaOffset = (UWORD)(out1->ulIaRegSum >> (ADC_CALIB_INDEX));
                    cof->uwIbOffset = (UWORD)(out1->ulIbRegSum >> (ADC_CALIB_INDEX));
                    cof->uwIcOffset = (UWORD)(out1->ulIcRegSum >> (ADC_CALIB_INDEX));

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

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

                    out1->uwADCCalibCt = 0;
                    out1->blADCCalibFlg = TRUE;
                    out2->uwADCCalibCt = 0;
                    out2->blADCCalibFlg = TRUE;
                }
            }
            else
            {
            	//do noting
            }
        }
    }
}
#endif
/***************************************************************
 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(const ADC_COF *cof, ADC_DOWN_OUT *out) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改，后续避免" */
{
    UWORD uwIpeakPu;

    if(cp_stFlg.CurrentSampleModelSelect == COMBINATION)
    {
        out->uwIaReg = iAdc_GetResultPointer(1)[HW_ADC_IA_CH];
        out->uwIbReg = iAdc_GetResultPointer(1)[HW_ADC_IB_CH];
        out->uwIcReg = iAdc_GetResultPointer(1)[HW_ADC_IC_CH];

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

    /* 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(const ADC_COF *cof, ADC_UP_OUT *out)
{
    /* Register value */
    out->uwVdcReg = iAdc_GetResultPointer(0)[HW_ADC_UDC_CH]; 
    out->uwU6VReg = iAdc_GetResultPointer(0)[HW_ADC_U6V_CH]; 
    out->uwU5VReg = iAdc_GetResultPointer(0)[HW_ADC_U5V_CH]; 
    out->PCBTempReg = iAdc_GetResultPointer(0)[HW_ADC_PCBTEMP_CH]; 
    out->TorqTempReg = iAdc_GetResultPointer(0)[HW_ADC_MOTTEMP_CH]; 
    out->uwU12VReg = iAdc_GetResultPointer(0)[HW_ADC_U12V_CH]; 
    out->uwThrottleReg = iAdc_GetResultPointer(0)[HW_ADC_THRO_CH]; 

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

    out->uwU6VPu = (UWORD)((ULONG)out->uwU6VReg * cof->uwU6VReg2Pu >> 10);    // Q14=Q24-Q10;
    out->uwU5VPu = (UWORD)((ULONG)out->uwU5VReg * cof->uwU5VReg2Pu >> 10);    // Q14=Q24-Q10;
    out->uwU12VPu = (UWORD)((ULONG)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 = -(SWORD)((((SWORD)iAdc_GetResultPointer(2)[HW_ADC_IDC_CH] - (SWORD)cof->uwIdcOffset) * (SLONG)cof->uwCurIdcReg2Pu) >> 10); // Q14=Q24-Q10
            break;
        case SampleB:
            out->swCalibIbPu = -(SWORD)((((SWORD)iAdc_GetResultPointer(2)[HW_ADC_IDC_CH] - (SWORD)cof->uwIdcOffset) * (SLONG)cof->uwCurIdcReg2Pu) >> 10); // Q14=Q24-Q10
            break;
        case SampleC:
            out->swCalibIcPu = -(SWORD)((((SWORD)iAdc_GetResultPointer(2)[HW_ADC_IDC_CH] - (SWORD)cof->uwIdcOffset) * (SLONG)cof->uwCurIdcReg2Pu) >> 10); // Q14=Q24-Q10
            break;
        default:
            break;
        }
    }

    ////////////////// PCB TEMP//////////////////////////////////////////////////////
    if(out->PCBTempReg != 0)
    {
        out->PCBTempR = (UWORD)((ULONG)4096 * PCB_TEMP_SAMPLER / out->PCBTempReg - PCB_TEMP_SAMPLER); // Q14=Q24-Q10;
    }
    PcbTempCal((SWORD)out->PCBTempR);
    out->PCBTemp = tmp_PcbTemp;
}

static SWORD adc_pvt_swSingleReg = 0;
static SLONG adc_pvt_slRdsonReg = 0;
static LPF_OUT adc_pvt_stRdsonCoefLpf = {.slY.sw.hi = 1024, .slY.sw.low = 0};
static BOOL adc_pvt_blCalGainFlg = FALSE;
static ULONG adc_pvt_ulGainTemp1 = 0;
static ULONG adc_pvt_ulIaAbsPu, adc_pvt_ulIbAbsPu, adc_pvt_ulIcAbsPu, adc_pvt_ulIPeakPu;
void adc_voSRCalibration(ADC_COF *cof, const 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(adc_pvt_swSingleReg > up_out->swCalibIaPu)
            {
              adc_pvt_swSingleReg = up_out->swCalibIaPu;
            }
            if(adc_pvt_slRdsonReg > down_out->slSampIaPu)
            {
              adc_pvt_slRdsonReg = down_out->slSampIaPu;
            }
            
            break;
        case SampleB:
            if(adc_pvt_swSingleReg > up_out->swCalibIbPu)
            {
              adc_pvt_swSingleReg = up_out->swCalibIbPu;
            }
            if(adc_pvt_slRdsonReg > down_out->slSampIbPu)
            {
              adc_pvt_slRdsonReg = down_out->slSampIbPu;
            }
            break;
        case SampleC:
            if(adc_pvt_swSingleReg > up_out->swCalibIcPu)
            {
              adc_pvt_swSingleReg = up_out->swCalibIcPu;
            }    
            if(adc_pvt_slRdsonReg > down_out->slSampIcPu)
            {
              adc_pvt_slRdsonReg = down_out->slSampIcPu;
            }
            break;
        default:
            break;
        }
      
        adc_pvt_blCalGainFlg = TRUE;
    }
    else
    {         
         ULONG ulOverflowCurPu = (ULONG)(4095 - cof->uwIaOffset) * cof->uwCurReg2Pu >> 10;

         if(scm_uwSpdFbkLpfAbsPu < 2500)
         {
            adc_pvt_ulIaAbsPu = abs(down_out->slSampIaPu);
            adc_pvt_ulIbAbsPu = abs(down_out->slSampIbPu);
            adc_pvt_ulIcAbsPu = abs(down_out->slSampIcPu);
            
            adc_pvt_ulIPeakPu = adc_pvt_ulIaAbsPu > adc_pvt_ulIbAbsPu ? adc_pvt_ulIaAbsPu : adc_pvt_ulIbAbsPu;
            down_out->ulISamplePeakPu = adc_pvt_ulIcAbsPu > adc_pvt_ulIPeakPu ? adc_pvt_ulIcAbsPu : adc_pvt_ulIPeakPu;

            if(down_out->ulISamplePeakPu > 32767)  
            {
                adc_pvt_ulGainTemp1 = 780;     ///< Rdson电流采样溢出SWORD时校准系数需小于1024
                adc_pvt_stRdsonCoefLpf.slY.sw.hi = (SWORD)adc_pvt_ulGainTemp1;  ///< 系数立刻变化，不经过滤波，防止down_out->swIaPu溢出
            }
            // else if(down_out->ulISamplePeakPu > 25800) ///<  25800 = 32767 / (1300 / 1024)
            // {
            //     adc_pvt_ulGainTemp1 = 1024;
            //     adc_pvt_stRdsonCoefLpf.slY.sw.hi = (SWORD)adc_pvt_ulGainTemp1;
            // }
            else
            {
                adc_pvt_ulGainTemp1 = 1024;    ///< 允许其他数值，但大于1024需注意溢出SWORD
            }

            cof->blCalibCalFlag = FALSE;
            adc_pvt_blCalGainFlg = FALSE;
         }
         else
         {
            if(adc_pvt_blCalGainFlg)
            {
                if(adc_pvt_slRdsonReg != 0 && abs(adc_pvt_slRdsonReg) < ulOverflowCurPu)
                {
                    adc_pvt_ulGainTemp1 = (SLONG)((SLONG)adc_pvt_swSingleReg << 10) / (SLONG)adc_pvt_slRdsonReg;
                }
                else if(abs(adc_pvt_slRdsonReg) >= ulOverflowCurPu)
                {
                    adc_pvt_ulGainTemp1 = 780;     ///< Rdson电流采样削顶时不再校准电流，强制输出为119A防止溢出，尽快报出过流故障
                    adc_pvt_stRdsonCoefLpf.slY.sw.hi = (SWORD)adc_pvt_ulGainTemp1;
                }
                else 
                {
                    // do nothing
                }
                                
                if(adc_pvt_ulGainTemp1 > cof->uwCalibcoefMax)
                {
                    adc_pvt_ulGainTemp1 = cof->uwCalibcoefMax;
                }
                else if(adc_pvt_ulGainTemp1 < cof->uwCalibcoefMin)
                {
                    adc_pvt_ulGainTemp1 = cof->uwCalibcoefMin;
                }
                else
                {
                    //do nothing
                }

                cof->blCalibCalFlag = TRUE;
                adc_pvt_blCalGainFlg = FALSE;
            }
            else 
            {
                adc_pvt_swSingleReg = 0;
                adc_pvt_slRdsonReg = 0;
            }
            
        }
                  
        mth_voLPFilter((SWORD)adc_pvt_ulGainTemp1, &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_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_stDownOut.ulISamplePeakPu = 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_stUpOut.uwADCCalibCt = 0;
    adc_stUpOut.blADCCalibFlg = FALSE;
    adc_stUpOut.swIPMTempCe = 0;

    adc_pvt_swSingleReg = 0;
    adc_pvt_slRdsonReg = 0;
    adc_pvt_stRdsonCoefLpf.slY.sw.hi = 1024;
    adc_pvt_stRdsonCoefLpf.slY.sw.low = 0;
    adc_pvt_blCalGainFlg = FALSE;
    adc_pvt_ulGainTemp1 = 0;
    adc_pvt_ulIaAbsPu = 0;
    adc_pvt_ulIbAbsPu = 0;
    adc_pvt_ulIcAbsPu = 0;
    adc_pvt_ulIPeakPu = 0;
}

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

/************************************************************************
 Copyright (c) 2018 Welling Motor Technology(Shanghai) Co. Ltd.
 All rights reserved.
*************************************************************************/
#ifdef _ADCDRV_C_
#undef _ADCDRV_C_ /* parasoft-suppress MISRA2004-19_6 "本项目中无法更改，后续避免使用" */
#endif
/*************************************************************************
 End of this File (EOF)!
 Do not put anything after this part!
*************************************************************************/