csu3_am335x/EVSE/Projects/DM30/Apps/Module_PsuComm.c

#include "Module_PsuComm.h"

#define ARRAY_SIZE(A)       (sizeof(A) / sizeof(A[0]))
#define PASS                1
#define FAIL                -1
#define YES                 1
#define NO                  0
#define DERATING            30
#define ELEMENT_NOT_FIND    255
#define CHK_VOL_RANGE       20
#define CHK_CUR_RANGE       10
#define DERATING_RANGE      100
#define ZERO_CURRENT        0
#define ZERO_VOLTAGE        10
#define STOP_CURRENT        30
#define PSU_MIN_CUR         100

struct SysConfigAndInfo         *ShmSysConfigAndInfo;
struct StatusCodeData           *ShmStatusCodeData;
struct PsuData                  *ShmPsuData;

bool libInitialize = false;
byte getAvailableCapOffset = 5;
byte deratingKeepCount = 0;

float carReqVol = 0;
float carReqCur = 0;
float evseOutVol = 0;
float evseOutCur = 0;

void PRINTF_FUNC(char *string, ...);

int StoreLogMsg(const char *fmt, ...);
#define DEBUG_INFO(format, args...) StoreLogMsg("[%s:%d][%s][Info] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_WARN(format, args...) StoreLogMsg("[%s:%d][%s][Warn] "format, __FILE__, __LINE__, __FUNCTION__, ##args)
#define DEBUG_ERROR(format, args...) StoreLogMsg("[%s:%d][%s][Error] "format, __FILE__, __LINE__, __FUNCTION__, ##args)

unsigned long GetTimeoutValue(struct timeval _sour_time);

unsigned long GetTimeoutValue(struct timeval _sour_time)
{
    struct timeval _end_time;
    gettimeofday(&_end_time, NULL);

    return 1000000 * (_end_time.tv_sec - _sour_time.tv_sec) + _end_time.tv_usec - _sour_time.tv_usec;
}

int StoreLogMsg(const char *fmt, ...)
{
    char Buf[4096+256];
    char buffer[4096];
    time_t CurrentTime;
    struct tm *tm;
    va_list args;

    va_start(args, fmt);
    int rc = vsnprintf(buffer, sizeof(buffer), fmt, args);
    va_end(args);

    memset(Buf,0,sizeof(Buf));
    CurrentTime = time(NULL);
    tm=localtime(&CurrentTime);
    sprintf(Buf,"echo \"%04d-%02d-%02d %02d:%02d:%02d - %s\" >> /Storage/SystemLog/[%04d.%02d]SystemLog",
            tm->tm_year+1900,tm->tm_mon+1,tm->tm_mday,tm->tm_hour,tm->tm_min,tm->tm_sec,
            buffer,
            tm->tm_year+1900,tm->tm_mon+1);
    system(Buf);

    return rc;
}

void PRINTF_FUNC(char *string, ...)
{
    va_list args;
    char buffer[4096];
    va_start(args, string);
    vsnprintf(buffer, sizeof(buffer), string, args);
    va_end(args);

    if (DEBUG)
        printf("%s \n", buffer);
    else
        DEBUG_INFO("%s \n", buffer);
}
//=================================
// Common routine
//=================================
size_t FindIndex(const int a[], size_t size, int value, byte group)
{
    size_t index = 0;

    while ( index < size && a[index] != value ) ++index;
    return (index == size ? ELEMENT_NOT_FIND : group);
}

byte FindTargetGroup(byte address)
{
    byte _group = ELEMENT_NOT_FIND;

    if (ShmPsuData->GroupCount == 1)
        _group = 0;
    else
    {
        _group = FindIndex(connector_1, ShmPsuData->PsuGroup[0].GroupPresentPsuQuantity, address, 0);

        if (_group == ELEMENT_NOT_FIND)
            _group = FindIndex(connector_2, ShmPsuData->PsuGroup[1].GroupPresentPsuQuantity, address, 1);
    }

    return _group;
}

bool IsOverModuleCount(byte count)
{
    bool result = false;

    if (count >= ShmPsuData->SystemPresentPsuQuantity)
        result = true;

    return result;
}
//=================================
// Save data to share memory Function
//=================================
bool FindChargingInfoData(byte target, struct ChargingInfoData **chargingData)
{
    for (byte index = 0; index < CHAdeMO_QUANTITY; index++)
    {
        if (ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index].Index == target)
        {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.ChademoChargingData[index];
            return true;
        }
    }

    for (byte index = 0; index < CCS_QUANTITY; index++)
    {
        if (ShmSysConfigAndInfo->SysInfo.CcsChargingData[index].Index == target)
        {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.CcsChargingData[index];
            return true;
        }
    }

    for (byte index = 0; index < GB_QUANTITY; index++)
    {
        if (ShmSysConfigAndInfo->SysInfo.GbChargingData[index].Index == target)
        {
            chargingData[target] = &ShmSysConfigAndInfo->SysInfo.GbChargingData[index];
            return true;
        }
    }

    return false;
}

//=================================
// Alarm code mapping to share memory Function
//=================================
// 檢查 Byte 中某個 Bit 的值
// _byte : 欲改變的 byte
// _bit : 該 byte 的第幾個 bit
unsigned char mask_table[] = { 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80 };
unsigned char DetectBitValue(unsigned char _byte, unsigned char _bit)
{
    return ( _byte & mask_table[_bit] ) != 0x00;
}

void AbnormalStopAnalysis(byte gun_index, int errCode)
{
    for (char i = 0; i < 3; i++)
    {
        unsigned char byteIndex = (errCode >> (8 * i)) & 0xff;

        for (char bitIndex = 0; bitIndex < 8; bitIndex++)
        {
            if(DetectBitValue(byteIndex , bitIndex) == 1)
            {
                switch(byteIndex)
                {
                    case 0:
                    {
                        if (bitIndex == 0)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuOutputShortCircuit = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = YES;
                    }
                        break;
                case 1:
                    {
                        if (bitIndex == 1)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFailureAlarm = YES;
                        else if (bitIndex == 2)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuProtectionAlarm = YES;
                        else if (bitIndex == 3)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFanFailureAlarm = YES;
                        else if (bitIndex == 4)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuCriticalPointOTP = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = YES;
                    }
                    break;
                case 2:
                    {
                        if (bitIndex == 0)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuPowerLimitedState = YES;
                        else if (bitIndex == 1)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDuplicateID = YES;
                        else if (bitIndex == 2)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseOnputImbalance = YES;
                        else if (bitIndex == 3)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = YES;
                        else if (bitIndex == 4)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = YES;
                        else if (bitIndex == 5)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputUVP = YES;
                        else if (bitIndex == 6)
                            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputOVP = YES;
                    }
                    break;
                }
            }
//          else
//          {
//              switch (byteIndex) {
//              case 0: {
//                  if (bitIndex == 0)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuOutputShortCircuit = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = NO;
//              }
//                  break;
//              case 1: {
//                  if (bitIndex == 1)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFailureAlarm = NO;
//                  else if (bitIndex == 2)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuProtectionAlarm = NO;
//                  else if (bitIndex == 3)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuFanFailureAlarm = NO;
//                  else if (bitIndex == 4)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuCriticalPointOTP = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDcSideShutDown = NO;
//              }
//                  break;
//              case 2: {
//                  if (bitIndex == 1)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuDuplicateID = NO;
//                  if (bitIndex == 2)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseOnputImbalance = NO;
//                  else if (bitIndex == 3)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = NO;
//                  else if (bitIndex == 4)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuThreePhaseInputInadequate = NO;
//                  else if (bitIndex == 5)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputUVP = NO;
//                  else if (bitIndex == 6)
//                      ShmStatusCodeData->AlarmCode.AlarmEvents.bits.PsuInputOVP = NO;
//              }
//                  break;
//              }
//          }
        }
    }
}

//=================================
// Callback Function
//=================================
void GetStatusCallback(byte group, byte address, byte temp, int alarm)
{
    if (IsOverModuleCount(address))
        return;

    byte group1 = FindTargetGroup(address);

    if (group1 == 1)
        address -= ShmPsuData->PsuGroup[group1 - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp1 = temp;
    ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp2 = temp;
    ShmPsuData->PsuGroup[group1].PsuModule[address].CriticalTemp3 = temp;
    ShmPsuData->PsuGroup[group1].PsuModule[address].ExletTemp = temp;
    ShmPsuData->PsuGroup[group1].PsuModule[address].AlarmCode = alarm;
    AbnormalStopAnalysis(group1, alarm);
    //printf("alarm = %d \n", alarm);
}

void GetModuleCountCallback(byte group, byte count)
{
    if (group == SYSTEM_CMD)
        ShmPsuData->SystemPresentPsuQuantity = count;
    else
    {
        ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity = count;
    }
}

void GetMaxPowerAndCur(unsigned char mode, int ratingCur, int *pow, int *cur)
{
    unsigned short maxCurrent = ShmSysConfigAndInfo->SysConfig.MaxChargingCurrent * 10;
    unsigned short maxPower = ShmSysConfigAndInfo->SysConfig.MaxChargingPower * 10;

    if (mode == _MAIN_CHARGING_MODE_AVER)
    {
        maxCurrent /= 2;
        maxPower /= 2;
    }

    if (maxPower != 0 && maxPower <= *pow)
        *pow = maxPower;

    if (maxCurrent != 0 && maxCurrent <= *cur)
        *cur = maxCurrent;

    if (ratingCur != 0 && ratingCur <= *cur)
        *cur = ratingCur;
}

void GetAvailableCapCallback(byte address, short maxVol, short minVol, short maxCur, short totalPow)
{
    int _groupPower = 0, _groupCurrent = 0;
    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    if (chargingInfo[group]->DeratingChargingCurrent == 0)
        ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent = PSU_MIN_CUR;
    else
        ShmPsuData->PsuGroup[group].PsuModule[address].AvailableCurrent = maxCur;

    ShmPsuData->PsuGroup[group].PsuModule[address].AvailablePower = totalPow;

    for (byte index = 0; index < ShmPsuData->PsuGroup[group].GroupPresentPsuQuantity; index++)
    {
        _groupCurrent += ShmPsuData->PsuGroup[group].PsuModule[index].AvailableCurrent;
        _groupPower += ShmPsuData->PsuGroup[group].PsuModule[index].AvailablePower;
    }

    // 各群得到最大輸出能力 (電流、Power)
    ShmPsuData->PsuGroup[group].GroupAvailableCurrent = _groupCurrent;
    ShmPsuData->PsuGroup[group].GroupAvailablePower = _groupPower;

    chargingInfo[group]->MaximumChargingVoltage = maxVol;

    int _power = 0, _current = 0, _ratingcurrent = 0;
    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
    {
        _power += ShmPsuData->PsuGroup[index].GroupAvailablePower;
        _current += ShmPsuData->PsuGroup[index].GroupAvailableCurrent;
        _ratingcurrent += chargingInfo[address]->DeratingChargingCurrent;
    }

    ShmPsuData->SystemAvailableCurrent = _current;
    ShmPsuData->SystemAvailablePower = _power;

    GetMaxPowerAndCur(_MAIN_CHARGING_MODE_MAX, _ratingcurrent, &_power, &_current);

    // 雙槍單模時，同步雙槍資訊
    if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
    {
        byte _count = CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY;

        for (byte count = 0; count < _count; count++)
        {
            chargingInfo[count]->MaximumChargingVoltage = maxVol;
            chargingInfo[count]->AvailableChargingCurrent = _current;
            chargingInfo[count]->AvailableChargingPower = _power;
        }
    }
    else
    {
        chargingInfo[group]->AvailableChargingCurrent = _current;
        chargingInfo[group]->AvailableChargingPower = _power;
    }
}

void GetFwCallback(byte address, short dcSwVer, short pfcSwVer, short hwVer)
{
    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;
    sprintf((char *)ShmPsuData->PsuGroup[group].PsuModule[address].FwVersion, "DC %d.%02d", (dcSwVer & 0xFF00) >> 8, dcSwVer & 0xFF);
    //DEBUG_INFO("fw Ver. = %s \n", ShmPsuData->PsuGroup[group].PsuModule[address].FwVersion);
}

void GetInputVoltageCallback(byte address, unsigned short vol1, unsigned short vol2, unsigned short vol3)
{
    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL1 = vol1;
    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL2 = vol2;
    ShmPsuData->PsuGroup[group].PsuModule[address].InputVoltageL3 = vol3;
}

void GetPresentOutputCallback(byte group, unsigned short outVol, unsigned short outCur)
{
    unsigned short outputVol = outVol;
    unsigned short outputCur = outCur;

    // PSU Group - 電壓
    ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage = outputVol;
    // PSU Group - 電流
    ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent = outputCur;

    if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
    {
        outputVol = 0;
        outputCur = 0;

        for (byte index = 0; index < ShmPsuData->GroupCount; index++)
        {
            if (ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage > outputVol)
                outputVol = ShmPsuData->PsuGroup[index].GroupPresentOutputVoltage;

            outputCur += ShmPsuData->PsuGroup[index].GroupPresentOutputCurrent;
        }

        // 黑白機
        if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
        {
            byte _count = CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY;

            for (byte count = 0; count < _count; count++)
            {
                // EVSE - 電壓
                chargingInfo[count]->PresentChargingVoltage = outputVol;
                // EVSE - 電流
                chargingInfo[count]->PresentChargingCurrent = outputCur;
            }
        }

        if ((chargingInfo[group]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[group]->SystemStatus <= S_CHARGING) ||
            (chargingInfo[group]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[group]->SystemStatus <= S_CCS_PRECHARGE_ST1))
        {
            // EVSE - 電壓
            chargingInfo[group]->PresentChargingVoltage = outputVol;
            // EVSE - 電流
            chargingInfo[group]->PresentChargingCurrent = outputCur;
        }
    }
    else
    {
        // EVSE - 電壓
        chargingInfo[group]->PresentChargingVoltage = ShmPsuData->PsuGroup[group].GroupPresentOutputVoltage;
        // EVSE - 電流
        chargingInfo[group]->PresentChargingCurrent = ShmPsuData->PsuGroup[group].GroupPresentOutputCurrent;
    }

//  PRINTF_FUNC("Gun_%d, PresentChargingCurrent = %f \n", group,
//          chargingInfo[group]->PresentChargingCurrent);
}

void GetFanSpeedCallback(byte address, unsigned int fanSpeed)
{
    if (IsOverModuleCount(address))
        return;

    byte group = FindTargetGroup(address);

    if (group == 1)
        address -= ShmPsuData->PsuGroup[group - 1].GroupPresentPsuQuantity;

    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_1 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_2 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_3 = fanSpeed;
    ShmPsuData->PsuGroup[group].PsuModule[address].FanSpeed_4 = fanSpeed;
}

void GetIavailableCallback(byte address, unsigned short Iavail, unsigned short Vext)
{
    //PRINTF_FUNC("GetIavailableCallback address_%d, Vext = %d, Iavail = %d \n", address, Vext, Iavail);

    bool isPass = true;

    if (Iavail == 0)
    {
        for (byte count = 0; count < 2; count++)
        {
            chargingInfo[address]->SampleChargingCur[count] = Iavail;
        }
    }
    else
    {
        for (byte count = 0; count < 2; count++)
        {
            if (chargingInfo[address]->SampleChargingCur[count] == 0)
            {
                chargingInfo[address]->SampleChargingCur[count] = Iavail;
                return;
            }
            else
            {
                if (chargingInfo[address]->SampleChargingCur[count] != Iavail)
                {
                    chargingInfo[address]->SampleChargingCur[count] = Iavail;
                    isPass = false;
                    continue;
                }
            }
        }
    }

    if (isPass)
    {
        chargingInfo[address]->DeratingChargingCurrent = Iavail;
    }
}

//==========================================
// Init all share memory
//==========================================
int InitShareMemory()
{
    int result = PASS;
    int MeterSMId;

    //creat ShmSysConfigAndInfo
    if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmget ShmSysConfigAndInfo NG %d \n");
        #endif
        result = FAIL;
    }
    else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmat ShmSysConfigAndInfo NG \n");
        #endif
        result = FAIL;
     }
    else
    {}

    //creat ShmStatusCodeData
    if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmget ShmStatusCodeData NG \n");
        #endif
        result = FAIL;
    }
    else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmat ShmStatusCodeData NG \n");
        #endif
        result = FAIL;
    }
    else
    {}

    //creat ShmPsuData
    if ((MeterSMId = shmget(ShmPsuKey, sizeof(struct PsuData),  0777)) < 0)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmget ShmPsuData NG \n");
        #endif
        result = FAIL;
    }
    else if ((ShmPsuData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("shmat ShmPsuData NG \n");
        #endif
        result = FAIL;
     }
    memset(ShmPsuData,0,sizeof(struct PsuData));

    return result;
}

//================================================
// Main process
//================================================
void InitialPsuData()
{
    ShmPsuData->SystemPresentPsuQuantity = 0;

    for (byte _groupCount = 0; _groupCount < ARRAY_SIZE(ShmPsuData->PsuGroup);  _groupCount++)
    {
        ShmPsuData->PsuGroup[_groupCount].GroupPresentPsuQuantity = 0;
        ShmPsuData->PsuGroup[_groupCount].GroupAvailablePower = 0;
        ShmPsuData->PsuGroup[_groupCount].GroupAvailableCurrent = 0;
    }

    ShmPsuData->Work_Step = _INIT_PSU_STATUS;
}

void Initialization()
{
    bool isPass = false;
    while(!isPass)
    {
        isPass = true;
        for (byte _index = 0; _index < _gunCount; _index++)
        {
            if (!FindChargingInfoData(_index, &chargingInfo[0]))
            {
                DEBUG_ERROR("EvComm (main) : FindChargingInfoData false \n");
                isPass = false;
                break;
            }
        }
    }

    if (ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf == YES)
        ShmPsuData->GroupCount = 1;
    else
        ShmPsuData->GroupCount = _gunCount;
}

int main(void)
{
    PRINTF_FUNC("Psu Task boot .... \n");
    if(InitShareMemory() == FAIL)
    {
        #ifdef SystemLogMessage
        DEBUG_ERROR("InitShareMemory NG\n");
        #endif
        if(ShmStatusCodeData != NULL)
        {
            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = 1;
        }
        sleep(5);
        return 0;
    }

    PRINTF_FUNC("InitShareMemory OK\n");

    // register callback function
    RefreshStatus(&GetStatusCallback);
    RefreshModuleCount(&GetModuleCountCallback);
    RefreshAvailableCap(&GetAvailableCapCallback);
    RefreshFwVersion(&GetFwCallback);
    RefreshInputVol(&GetInputVoltageCallback);
    RefreshGetOutput(&GetPresentOutputCallback);
    RefreshFanInfo(&GetFanSpeedCallback);
    RefreshIavailable(&GetIavailableCallback);

    _gunCount = GUN_COUNT;
    // initial object
    InitialPsuData();
    Initialization();
    libInitialize = InitialCommunication();
    byte isInitialComp = NO;
    PRINTF_FUNC("ALTERNATIVE_CONG = %d \n", ShmSysConfigAndInfo->SysInfo.IsAlternatvieConf);
    //main loop
    while (libInitialize)
    {
        // 斷電狀態
        if (ShmSysConfigAndInfo->SysInfo.AcContactorStatus == NO)
        {
            //一但 AC Off PSU 斷電全部的 PSU Group ID 會全部清 0
            if (!isInitialComp)
            {
                InitialPsuData();
                ShmPsuData->Work_Step = INITIAL_START;
                isInitialComp = YES;
            }
            sleep(1);
            continue;
        }
        else
            isInitialComp = NO;

        // 自檢失敗
        if (ShmPsuData->Work_Step == _NO_WORKING)
        {
            PRINTF_FUNC("== PSU == self test fail. \n");
            sleep(5);
        }

        switch(ShmPsuData->Work_Step)
        {
            case INITIAL_START:
            {
                PRINTF_FUNC("== PSU == INITIAL_START \n");
                gettimeofday(&_cmdSubPriority_time, NULL);
                sleep(5);
                ShmPsuData->Work_Step = GET_PSU_COUNT;
            }
                break;
            case GET_PSU_COUNT:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;
                byte moduleCount = 0;

                if (time > 2000)
                {
                    PRINTF_FUNC("== PSU == GET_PSU_COUNT = %d \n", ShmPsuData->GroupCount);
                    if (ShmPsuData->GroupCount == 0)
                        ShmPsuData->GroupCount = 1;
                    // 分別取各群模組數量
                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        // 總和各群模組數量
                        moduleCount += ShmPsuData->PsuGroup[index].GroupPresentPsuQuantity;

                        // 取各群模組數量
                        GetModuleCount(index);

                        // 取版號
                        GetModuleVer(index);
                    }

                    // 發送取得目前全部模組數量
                    GetModuleCount(SYSTEM_CMD);

                    // 判斷系統數量與各群數量一致
                    if(moduleCount == ShmPsuData->SystemPresentPsuQuantity && moduleCount > 0)
                    {
                        PRINTF_FUNC("Psu Count = %d \n", moduleCount);
                        if (ShmSysConfigAndInfo->SysInfo.BootingStatus == BOOTTING)
                        {
                            // 電樁在 Booting 的狀態 - 自檢
                            PRINTF_FUNC("== PSU == GET_SYS_CAP \n");
                            ShmPsuData->Work_Step = GET_SYS_CAP;
                        }
                        else
                        {
                            PRINTF_FUNC("== PSU == _WORK_CHARGING \n");
                            ShmPsuData->Work_Step = _WORK_CHARGING;
                        }
                    }

                    _getCapDelayCount = 3;
                    gettimeofday(&_cmdSubPriority_time, NULL);
                }
            }
                break;
            case GET_SYS_CAP:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;

                if (time > 1000)
                {
                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        // Pooling Status
                        GetStatus(index);

                        // 取系統總輸出能力
                        GetModuleCap(index);
                    }
                    _getCapDelayCount--;
                    gettimeofday(&_cmdSubPriority_time, NULL);
                }

                // 判斷系統輸出額定功率與電流
                if (ShmPsuData->SystemAvailablePower > 0 && ShmPsuData->SystemAvailableCurrent > 0 &&
                        _getCapDelayCount <= 0)
                {
                    PRINTF_FUNC("SystemAvailableCurrent = %d, SystemAvailablePower = %d \n",
                            ShmPsuData->SystemAvailableCurrent, ShmPsuData->SystemAvailablePower);

                    PRINTF_FUNC("== PSU == BOOTING_COMPLETE \n");
                    ShmPsuData->Work_Step = BOOTING_COMPLETE;
                }
            }
                break;
            case BOOTING_COMPLETE:
            {
                sleep(1);
            }
                break;
            case _WORK_CHARGING:
            {
                int time = GetTimeoutValue(_cmdSubPriority_time) / 1000;
                // 低 Priority 的指令
                if (time > 1500)
                {
                    for (byte index = 0; index < ShmPsuData->GroupCount; index++)
                    {
                        // Pooling Status
                        GetStatus(index);

                        // 取系統總輸出能力
                        GetModuleCap(index);

                        // 取得模塊輸入電壓
                        GetModuleInput(index);

                        // 取得模塊輸出額定電流能力
                        GetModuleIavailable(index);
                    }

                    gettimeofday(&_cmdSubPriority_time, NULL);
                }

                for (byte groupIndex = 0; groupIndex < _gunCount; groupIndex++)
                {
                    GetModuleOutput(groupIndex);
                    // 針對各槍當前狀態，傳送需要回傳的資料指令
                    if (((chargingInfo[groupIndex]->SystemStatus >= S_PREPARING_FOR_EVSE && chargingInfo[groupIndex]->SystemStatus <= S_CHARGING) && chargingInfo[groupIndex]->RelayK1K2Status) ||
                            (chargingInfo[groupIndex]->SystemStatus >= S_CCS_PRECHARGE_ST0 && chargingInfo[groupIndex]->SystemStatus <= S_CCS_PRECHARGE_ST1))
                    {
                        if (chargingInfo[groupIndex]->EvBatterytargetVoltage > 0 &&
                            carReqVol != chargingInfo[groupIndex]->EvBatterytargetVoltage)
                        {
                            carReqVol = chargingInfo[groupIndex]->EvBatterytargetVoltage;
                            DEBUG_INFO("ev need vol = %f \n", chargingInfo[groupIndex]->EvBatterytargetVoltage);
                        }

                        if (chargingInfo[groupIndex]->EvBatterytargetCurrent > 0 &&
                            carReqCur != chargingInfo[groupIndex]->EvBatterytargetCurrent)
                        {
                            carReqCur = chargingInfo[groupIndex]->EvBatterytargetCurrent;
                            DEBUG_INFO("ev need cur = %f \n", chargingInfo[groupIndex]->EvBatterytargetCurrent);
                        }

                        if (chargingInfo[groupIndex]->FireChargingVoltage > 0 &&
                            evseOutVol != chargingInfo[groupIndex]->FireChargingVoltage)
                        {
                            evseOutVol = chargingInfo[groupIndex]->FireChargingVoltage;
                            PRINTF_FUNC("groupIndex = %d, evse output vol = %f \n", groupIndex,
                                    chargingInfo[groupIndex]->FireChargingVoltage);
                        }

                        if (chargingInfo[groupIndex]->PresentChargingCurrent > 0 &&
                            evseOutCur != chargingInfo[groupIndex]->PresentChargingCurrent)
                        {
                            evseOutCur = chargingInfo[groupIndex]->PresentChargingCurrent;
                            PRINTF_FUNC("groupIndex = %d, evse output cur = %f \n", groupIndex,
                                chargingInfo[groupIndex]->PresentChargingCurrent);
                        }

                        if (ShmPsuData->SystemAvailablePower > 0)
                        {
                            // 該充電槍的目標電壓與目標電流
                            PresentOutputVol(SYSTEM_CMD,
                                    chargingInfo[groupIndex]->EvBatterytargetVoltage,
                                    chargingInfo[groupIndex]->EvBatterytargetCurrent);

                            if (chargingInfo[groupIndex]->EvBatterytargetVoltage == 0)
                            {
                                SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                                FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                            }
                            else
                            {
                                SwitchPower(SYSTEM_CMD, PSU_POWER_ON);
                                FlashLed(SYSTEM_CMD, PSU_FLASH_ON);
                            }
                        }
                    }
                    else if (chargingInfo[groupIndex]->SystemStatus >= S_TERMINATING &&
                                chargingInfo[groupIndex]->SystemStatus <= S_COMPLETE)
                    {
                        if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX)
                        {
                            SwitchPower(SYSTEM_CMD, PSU_POWER_OFF);
                            FlashLed(SYSTEM_CMD, PSU_FLASH_NORMAL);
                        }
                        else if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER)
                        {
                            SwitchPower(groupIndex, PSU_POWER_OFF);
                            FlashLed(groupIndex, PSU_FLASH_NORMAL);
                        }
                    }
                }
                    break;
            }
        }
        usleep(20000);
    }
    return FAIL;
}