csu3_am335x/EVSE/Projects/DD360Audi/Apps/ReModule_PrimaryComm.c

#include    <sys/time.h>
#include    <sys/timeb.h>
#include    <sys/types.h>
#include    <sys/stat.h>
#include    <sys/types.h>
#include    <sys/ioctl.h>
#include    <sys/socket.h>
#include    <sys/ipc.h>
#include    <sys/shm.h>
#include    <sys/shm.h>
#include    <sys/mman.h>
#include    <linux/wireless.h>
#include    <arpa/inet.h>
#include    <netinet/in.h>

#include    <unistd.h>
#include    <stdarg.h>
#include    <stdio.h>      /*標準輸入輸出定義*/
#include    <stdlib.h>     /*標準函數庫定義*/
#include    <unistd.h>     /*Unix 標準函數定義*/
#include    <fcntl.h>      /*檔控制定義*/
#include    <termios.h>    /*PPSIX 終端控制定義*/
#include    <errno.h>      /*錯誤號定義*/
#include    <errno.h>
#include    <string.h>
#include    <time.h>
#include    <ctype.h>
#include    <ifaddrs.h>
#include    <math.h>
#include    "../../define.h"
#include    "PrimaryComm.h"
#include    <stdbool.h>

#define ARRAY_SIZE(A)       (sizeof(A) / sizeof(A[0]))
#define PASS                1
#define FAIL                -1
#define YES                 1
#define NO                  0

typedef unsigned char       byte;

struct SysConfigAndInfo         *ShmSysConfigAndInfo;
struct StatusCodeData           *ShmStatusCodeData;
struct PrimaryMcuData           *ShmPrimaryMcuData;
#if defined METER_ENABLE //DS60-120 add
struct MeterInformation         *ShmCsuMeterData;
#endif //defined METER_ENABLE

void trim(char *s);
int mystrcmp(char *p1, char *p2);
void substr(char *dest, const char *src, unsigned int start, unsigned int cnt);
void split(char **arr, char *str, const char *del);

int Uart1Fd;
char *priPortName = "/dev/ttyS1";
Ver ver;
Gpio_in gpio_in;
Rtc rtc;

struct timeval _flash_time;
byte flash = NO;
byte gun_count; //DS60-120 add

byte _curDeviceStatus[3] = {0, 0, 0}; //DS60-120 add
byte _reCheckCount[3] = {0, 0, 0}; //DS60-120 add
struct ChargingInfoData *ChargingData[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY];

#if defined DD360 || defined DD360Audi
unsigned char ChillerSwitch; //DS60-120 remove
unsigned int ChillerOnTime; //DS60-120 remove
#endif //defined DD360 || defined DD360Audi

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)

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

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

    memset(Buf, 0, sizeof(Buf));
    ftime(&SeqEndTime);
    SeqEndTime.time = time(NULL);
    tm = localtime(&SeqEndTime.time);

    if (ShmSysConfigAndInfo->SysConfig.SwitchDebugFlag == YES) {
        sprintf(Buf, "%02d:%02d:%02d:%03d - %s",
                tm->tm_hour, tm->tm_min, tm->tm_sec, SeqEndTime.millitm, buffer);
        printf("%s \n", Buf);
    } else {
        sprintf(Buf, "echo \"%04d-%02d-%02d %02d:%02d:%02d:%03d - %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, SeqEndTime.millitm,
                buffer,
                tm->tm_year + 1900, tm->tm_mon + 1);
        system(Buf);
    }

    return rc;
}

int DiffTimeb(struct timeb ST, struct timeb ET)
{
    //return milli-second
    unsigned int StartTime, StopTime;

    StartTime = (unsigned int)ST.time;
    StopTime = (unsigned int)ET.time;
    return (StopTime - StartTime) * 1000 + ET.millitm - ST.millitm;
}

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

    DEBUG_INFO("%s \n", buffer);
}
//=================================
// Common routine
//=================================
char *getTimeString(void)
{
    char *result = malloc(21);
    time_t timep;
    struct tm *p;
    time(&timep);
    p = gmtime(&timep);

    sprintf(result, "[%04d-%02d-%02d %02d:%02d:%02d]", (1900 + p->tm_year), (1 + p->tm_mon), p->tm_mday, p->tm_hour, p->tm_hour, p->tm_sec);

    return result;
}

void trim(char *s)
{
    int i = 0, j, k, l = 0;

    while ((s[i] == ' ') || (s[i] == '\t') || (s[i] == '\n')) {
        i++;
    }

    j = strlen(s) - 1;
    while ((s[j] == ' ') || (s[j] == '\t') || (s[j] == '\n')) {
        j--;
    }

    if (i == 0 && j == strlen(s) - 1) { }
    else if (i == 0) { s[j + 1] = '\0'; }
    else {
        for (k = i; k <= j; k++) { s[l++] = s[k]; }
        s[l] = '\0';
    }
}

int mystrcmp(char *p1, char *p2)
{
    while (*p1 == *p2) {
        if (*p1 == '\0' || *p2 == '\0') {
            break;
        }
        p1++;
        p2++;
    }
    if (*p1 == '\0' && *p2 == '\0') {
        return (PASS);
    } else {
        return (FAIL);
    }
}

void substr(char *dest, const char *src, unsigned int start, unsigned int cnt)
{
    strncpy(dest, src + start, cnt);
    dest[cnt] = 0;
}

void split(char **arr, char *str, const char *del)
{
    char *s = strtok(str, del);

    while (s != NULL) {
        *arr++ = s;
        s = strtok(NULL, del);
    }
}

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

    //creat ShmSysConfigAndInfo
    if ((MeterSMId = shmget(ShmSysConfigAndInfoKey, sizeof(struct SysConfigAndInfo),  0777)) < 0) {
        result = FAIL;
    } else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        result = FAIL;
    }

    //creat ShmStatusCodeData
    if ((MeterSMId = shmget(ShmStatusCodeKey, sizeof(struct StatusCodeData),  0777)) < 0) {
        result = FAIL;
    } else if ((ShmStatusCodeData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        result = FAIL;
    }

    //creat ShmStatusCodeData
    if ((MeterSMId = shmget(ShmPrimaryMcuKey, sizeof(struct PrimaryMcuData), 0777)) < 0) {
        result = FAIL;
    } else if ((ShmPrimaryMcuData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        result = FAIL;
    }

#if defined METER_ENABLE //DS60-120 add
    if ((MeterSMId = shmget(ShmCsuMeterKey, sizeof(struct MeterInformation), IPC_CREAT | 0777)) < 0) {
        result = FAIL;
    } else if ((ShmCsuMeterData = shmat(MeterSMId, NULL, 0)) == (void *) - 1) {
        result = FAIL;
    }
#endif //defined METER_ENABLE

    return result;
}

//================================================
// Function
//================================================
void GetFwAndHwVersion()
{
    if (Query_FW_Ver(Uart1Fd, Addr.IoExtend, &ver) == PASS) {
        //PRINTF_FUNC("Primary FW Rev = %s \n", ver.Version_FW);
        strcpy((char *)ShmPrimaryMcuData->version, ver.Version_FW);
        strcpy((char *) ShmSysConfigAndInfo->SysInfo.CsuPrimFwRev, ver.Version_FW);
    }

    if (Query_HW_Ver(Uart1Fd, Addr.IoExtend, &ver) == PASS)
        ;//PRINTF_FUNC("Primary HW Rev  = %s \n", ver.Version_HW);
}

void GetInputGpioStatus()
{
    //PRINTF_FUNC("GetInputGpioStatus \n");
    if (Query_Gpio_Input(Uart1Fd, Addr.IoExtend, &gpio_in) == PASS) {
#if !defined DD360 && !defined DD360Audi
        //DS60-120 add
        if (_curDeviceStatus[_PRIMARY_CHECK_TAG_AC_CONTACT] != gpio_in.AC_Connector) {
            if (_reCheckCount[_PRIMARY_CHECK_TAG_AC_CONTACT] >= 3) {
                _curDeviceStatus[_PRIMARY_CHECK_TAG_AC_CONTACT] = gpio_in.AC_Connector;
                ShmSysConfigAndInfo->SysInfo.AcContactorStatus =
                    ShmPrimaryMcuData->InputDet.bits.AcContactorDetec =
                        gpio_in.AC_Connector;
            } else {
                _reCheckCount[_PRIMARY_CHECK_TAG_AC_CONTACT]++;
            }
        } else {
            _reCheckCount[_PRIMARY_CHECK_TAG_AC_CONTACT] = 0;
        }

        if (_curDeviceStatus[_PRIMARY_CHECK_TAG_MAIN_BREAKER] != gpio_in.AC_MainBreaker) {
            if (_reCheckCount[_PRIMARY_CHECK_TAG_MAIN_BREAKER] >= 3) {
                _curDeviceStatus[_PRIMARY_CHECK_TAG_MAIN_BREAKER] = gpio_in.AC_MainBreaker;
                ShmPrimaryMcuData->InputDet.bits.AcMainBreakerDetec = gpio_in.AC_MainBreaker;
            } else {
                _reCheckCount[_PRIMARY_CHECK_TAG_MAIN_BREAKER]++;
            }
        } else {
            _reCheckCount[_PRIMARY_CHECK_TAG_MAIN_BREAKER] = 0;
        }
        //ShmSysConfigAndInfo->SysInfo.AcContactorStatus = ShmPrimaryMcuData->InputDet.bits.AcContactorDetec = gpio_in.AC_Connector;
        //ShmPrimaryMcuData->InputDet.bits.AcMainBreakerDetec = gpio_in.AC_MainBreaker;
#else
        ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CcsLiquidChillerWaterLevelWarning = ~gpio_in.AC_Connector;
        ShmStatusCodeData->FaultCode.FaultEvents.bits.CcsLiquidChillerWaterLevelFault = ~gpio_in.AC_MainBreaker;
#endif //!defined DD360 && !defined DD360Audi

        ShmPrimaryMcuData->InputDet.bits.SpdDetec = gpio_in.SPD;
#if defined DD360 ||defined DD360Audi
        ShmPrimaryMcuData->InputDet.bits.DoorOpen = ~gpio_in.Door_Open;

        ShmPrimaryMcuData->InputDet.bits.Key0 = ~gpio_in.Key[0] & 0x01;
        ShmPrimaryMcuData->InputDet.bits.Key1 = ~gpio_in.Key[1] & 0x01;
        ShmPrimaryMcuData->InputDet.bits.Key2 = ~gpio_in.Key[2] & 0x01;
        ShmPrimaryMcuData->InputDet.bits.Key3 = ~gpio_in.Key[3] & 0x01;
#else
        ShmPrimaryMcuData->InputDet.bits.DoorOpen = gpio_in.Door_Open;
#endif //defined DD360 ||defined DD360Audi
        ShmPrimaryMcuData->InputDet.bits.Button1 = gpio_in.Button[0];
        ShmPrimaryMcuData->InputDet.bits.Button2 = gpio_in.Button[1];
        ShmPrimaryMcuData->InputDet.bits.EmergencyButton = gpio_in.Emergency_Btn;

        /*printf(" gpio_in.Key[0]~ gpio_in.Key[3]=%d, %d, %d, %d\n",
        ShmPrimaryMcuData->InputDet.bits.Key0 , ShmPrimaryMcuData->InputDet.bits.Key1,
        ShmPrimaryMcuData->InputDet.bits.Key2,ShmPrimaryMcuData->InputDet.bits.Key3);
        printf("ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CcsLiquidChillerWaterLevelWarning=%d\n", ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CcsLiquidChillerWaterLevelWarning);
        printf("ShmStatusCodeData->FaultCode.FaultEvents.bits.CcsLiquidChillerWaterLevelFault=%d\n", ShmStatusCodeData->FaultCode.FaultEvents.bits.CcsLiquidChillerWaterLevelFault);
        */
        //PRINTF_FUNC("left = %d \n", ShmPrimaryMcuData->InputDet.bits.Button1);
        //PRINTF_FUNC("right = %d \n", ShmPrimaryMcuData->InputDet.bits.Button2);
        //PRINTF_FUNC("ShmSysConfigAndInfo->SysInfo.AcContactorStatus = %d \n", ShmSysConfigAndInfo->SysInfo.AcContactorStatus);
#if !defined DD360 && !defined DD360Audi
        if (ShmPrimaryMcuData->InputDet.bits.AcMainBreakerDetec == YES) {
            DEBUG_ERROR("AC Mainbreaker occur. \n");
        }
#endif //!defined DD360 && !defined DD360Audi
    }
}

#if defined METER_ENABLE //DS60-120 add
void GetMeterConsumption(byte index)
{
    if (Query_Meter_value(Uart1Fd, Addr.IoExtend, &ShmCsuMeterData->_meter[index], &ShmCsuMeterData->isWorking, index) == PASS) {
        // meter value (Resolution) : 10
        if (!ShmCsuMeterData->isWorking) {
            //ShmStatusCodeData->AlarmCode.AlarmEvents.bits.MeterCommTimeout = YES;
            return;
        }

        if (ShmCsuMeterData->_meter[index].isCalculation == YES) {
//          printf("Meter%d, Comsumption = %.1f, curMeterValue = %.1f \n",
//                  index,
//                  ShmCsuMeterData->_meter[index].newMeterValue,
//                  ShmCsuMeterData->_meter[index].curMeterValue);

            if (ShmCsuMeterData->_meter[index].curMeterValue == 0) {
                ShmCsuMeterData->_meter[index].curMeterValue = ShmCsuMeterData->_meter[index].newMeterValue;
            } else {
                // 最大充時，兩個都會累加，但只輸出給一槍使用
                ShmCsuMeterData->_meter[index]._chargingValue += ShmCsuMeterData->_meter[index].newMeterValue - ShmCsuMeterData->_meter[index].curMeterValue;

                if (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_MAX ||
                        (ShmSysConfigAndInfo->SysInfo.MainChargingMode == _MAIN_CHARGING_MODE_AVER &&
                         ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_RELAY_A_TO_M)) {
                    for (byte subIndex = 0; subIndex < gun_count; subIndex++) {
                        if (ChargingData[subIndex]->SystemStatus == S_CHARGING) {
                            ShmCsuMeterData->_meter[subIndex]._curTotalCharging += ShmCsuMeterData->_meter[index].newMeterValue - ShmCsuMeterData->_meter[index].curMeterValue;
                            break;
                        }
                    }
                } else {
                    ShmCsuMeterData->_meter[index]._curTotalCharging += ShmCsuMeterData->_meter[index].newMeterValue - ShmCsuMeterData->_meter[index].curMeterValue;
                }

                ShmCsuMeterData->_meter[index].curMeterValue = ShmCsuMeterData->_meter[index].newMeterValue;
            }
        }
    }
}
#endif //defined METER_ENABLE

void SetOutputGpio(byte flash)
{
    Gpio_out gpio;
    gpio.Button_LED[0] = flash;
    gpio.Button_LED[1] = flash;

    gpio.System_LED[0] = 0x00;
    gpio.System_LED[1] = 0x00;
    gpio.System_LED[2] = 0x00;
    gpio.System_LED[3] = 0x00;

#if defined DD360 || defined DD360Audi
    if ((ChargingData[0]->PresentChargingCurrent) >= 150) {
        ChillerSwitch = 1;
        ChillerOnTime = time((time_t *)NULL);
    } else {
        if (ChillerSwitch == 1) {
            if ((ChargingData[0]->PresentChargingCurrent) >= 100) {
                ChillerSwitch = 1;
                ChillerOnTime = time((time_t *)NULL);
            } else {
                if ((time((time_t *)NULL) - ChillerOnTime) >= 600) {
                    ChillerSwitch = 0;
                } else {
                    ChillerSwitch = 1;
                }
            }
        } else {
            ChillerSwitch = 0;
        }
    }
    gpio.AC_Connector = ChillerSwitch;//Chiller ON/OFF Control, "0: Chiller disable, 1: Chiller enable"

    //printf("ChargingData[0]->PresentChargingCurrent=%f,ChargingData[1]->PresentChargingCurrent=%f,ChillerSwitch=%d\n",ChargingData[0]->PresentChargingCurrent,ChargingData[1]->PresentChargingCurrent,ChillerSwitch);
#else
    gpio.AC_Connector = 0x00;
#endif //defined DD360 || DD360Audi

    gpio.AC_Breaker = 0x00;

    if (Config_Gpio_Output(Uart1Fd, Addr.IoExtend, &gpio) == PASS) {
        //PRINTF_FUNC("SetOutputGpio sucessfully. %d \n", flash);
    } else {
        //PRINTF_FUNC("SetOutputGpio fail. \n");
    }
}

void SetRtcData()
{
    struct timeb csuTime;
    struct tm *tmCSU;

    ftime(&csuTime);
    tmCSU = localtime(&csuTime.time);
//  PRINTF_FUNC("Time : %04d-%02d-%02d %02d:%02d:%02d \n", tmCSU->tm_year + 1900,
//          tmCSU->tm_mon + 1, tmCSU->tm_mday, tmCSU->tm_hour, tmCSU->tm_min,
//          tmCSU->tm_sec);

    rtc.RtcData[0] = '0' + (tmCSU->tm_year + 1900) / 1000 % 10;
    rtc.RtcData[1] = '0' + (tmCSU->tm_year + 1900) / 100 % 10;
    rtc.RtcData[2] = '0' + (tmCSU->tm_year + 1900) / 10 % 10;
    rtc.RtcData[3] = '0' + (tmCSU->tm_year + 1900) / 1 % 10;

    rtc.RtcData[4] = '0' + (tmCSU->tm_mon + 1) / 10 % 10;
    rtc.RtcData[5] = '0' + (tmCSU->tm_mon + 1) / 1 % 10;

    rtc.RtcData[6] = '0' + (tmCSU->tm_mday) / 10 % 10;
    rtc.RtcData[7] = '0' + (tmCSU->tm_mday) / 1 % 10;

    rtc.RtcData[8] = '0' + (tmCSU->tm_hour) / 10 % 10;
    rtc.RtcData[9] = '0' + (tmCSU->tm_hour) / 1 % 10;

    rtc.RtcData[10] = '0' + (tmCSU->tm_min) / 10 % 10;
    rtc.RtcData[11] = '0' + (tmCSU->tm_min) / 1 % 10;

    rtc.RtcData[12] = '0' + (tmCSU->tm_sec) / 10 % 10;
    rtc.RtcData[13] = '0' + (tmCSU->tm_sec) / 1 % 10;

    if (Config_Rtc_Data(Uart1Fd, Addr.IoExtend, &rtc) == PASS) {
        //PRINTF_FUNC("SetRtc sucessfully. \n");
    } else {
        //PRINTF_FUNC("SetRtc fail. \n");
    }
}

void SetModelName()
{
    if (Config_Model_Name(Uart1Fd, Addr.IoExtend, ShmSysConfigAndInfo->SysConfig.ModelName) == PASS) {
    }
}

//================================================
// Main process
//================================================
int InitComPort()
{
    int fd;
    struct termios tios;

    fd = open(priPortName, O_RDWR);
    if (fd <= 0) {
#ifdef SystemLogMessage
        DEBUG_ERROR("open 407 Communication port NG \n");
#endif
        return -1;
    }
    ioctl (fd, TCGETS, &tios);
    tios.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
    tios.c_lflag = 0;
    tios.c_iflag = 0;
    tios.c_oflag = 0;
    tios.c_cc[VMIN] = 0;
    tios.c_cc[VTIME] = (unsigned char)1;
    tios.c_lflag = 0;
    tcflush(fd, TCIFLUSH);
    ioctl (fd, TCSETS, &tios);

    return fd;
}

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 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 1;
        }
    }

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

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

    return 0;
}

void Initialization() //DS60-120 add
{
    bool isPass = false;
    while (!isPass) {
        isPass = true;
        for (byte _index = 0; _index < gun_count; _index++) {
            if (!FindChargingInfoData(_index, &ChargingData[0])) {
                DEBUG_ERROR("EvComm (main) : FindChargingInfoData false \n");
                isPass = false;
                break;
            }
        }
        sleep(1);
    }
}

int main(void)
{
    if (InitShareMemory() == FAIL) {
        DEBUG_ERROR("InitShareMemory NG\r\n");
        if (ShmStatusCodeData != NULL) {
            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory = 1;
        }
        sleep(5);
        return 0;
    }

    //DS60-120 remove
    //for (byte _index = 0; _index < ShmSysConfigAndInfo->SysConfig.TotalConnectorCount; _index++) {
    //    if (!FindChargingInfoData(_index, &ChargingData[0])) {
    //        DEBUG_ERROR("FindChargingInfoData false \n");
    //        break;
    //    }
    //}
    ChillerSwitch = 0;
    Uart1Fd = InitComPort();
    //PRINTF_FUNC("407 Port id = %d \n", Uart1Fd);

    if (Uart1Fd < 0) {
#ifdef SystemLogMessage
        DEBUG_ERROR("InitComPort (Uart1 : AM3352 - STM32) NG");
#endif
        if (ShmStatusCodeData != NULL) {
            ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed = 1;
        }
        sleep(5);
        return 0;
    }

    SetRtcData();
    SetModelName();//DS60-120 add
    gettimeofday(&_flash_time, NULL);
    //DS60-120 add
    gun_count = ShmSysConfigAndInfo->SysConfig.TotalConnectorCount;
    Initialization();
    byte _count = 0;

    for (;;) {
        if (strcmp((char *)ShmSysConfigAndInfo->SysInfo.LcmHwRev, " ") == 0x00) {
            if ((GetTimeoutValue(_flash_time) / 1000) > 1000) {
                if (flash == NO) {
                    flash = YES;
                } else {
                    flash = NO;
                }
                SetOutputGpio(flash);
                gettimeofday(&_flash_time, NULL);
            }
        } else {
            if ((GetTimeoutValue(_flash_time) / 1000) > 5000) {
                if (flash == NO) {
                    flash = YES;
                }

                SetOutputGpio(flash);
                gettimeofday(&_flash_time, NULL);
            }
        }

        // 程序開始之前~ 必須先確定 FW 版本與硬體版本，確認後!!~ 該模組才算是真正的 Initial Comp.
        // 模組更新 FW 後，需重新做
        if (ShmPrimaryMcuData->SelfTest_Comp != PASS) {
            //PRINTF_FUNC("(407) Get Fw and Hw Ver. \n");
            GetFwAndHwVersion();
            sleep(1);
            ShmPrimaryMcuData->SelfTest_Comp = PASS;
        } else {
            GetInputGpioStatus();

#if defined METER_ENABLE //DS60-120 add
            if (ShmSysConfigAndInfo->SysConfig.ModelName[3] == 'M' ||
                    ShmSysConfigAndInfo->SysConfig.ModelName[3] == 'L') {
                GetMeterConsumption(_count);
                _count++;
                if (_count >= gun_count) {
                    _count = 0;
                }
            }
#endif //defined METER_ENABLE
        }

        usleep(100000);
    }

    return FAIL;
}