csu3_am335x/EVSE/Projects/DD360ComBox/Apps/ModulePrimary/Module_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 <stdbool.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 "../Log/log.h"
#include "../Define/define.h"
#include "../Config.h"
#include "../ShareMemory/shmMem.h"
#include "PrimaryComm.h"
#include "Module_PrimaryComm.h"

//------------------------------------------------------------------------------
//struct SysConfigAndInfo *ShmSysConfigAndInfo;
//struct StatusCodeData *ShmStatusCodeData;
static struct SysConfigData *pSysConfig = NULL;
static struct SysInfoData *pSysInfo = NULL;
static struct AlarmCodeData *pAlarmCode = NULL;
static struct FaultCodeData *pFaultCode = NULL;
static struct PrimaryMcuData *ShmPrimaryMcuData;
static DcCommonInfo *ShmDcCommonData = NULL;

const char *priPortName = "/dev/ttyS1";
uint8_t gun_count; //DS60-120 add

uint8_t EmgBtn_count = 0;
uint8_t Door_count = 0;
uint8_t EmgBtn_flag = 0;
uint8_t Door_flag = 0;
bool _isNeedChkTilt = FALSE;
struct timespec _tilt_time;
uint8_t tilt = NO;
bool _chargeSwitch;
//================================================
// Function
//================================================
bool GetFwAndHwVersion(int fd)
{
    Ver ver = {0};

    if (Query_FW_Ver(fd, OP_ADDR_IO_EXTEND, &ver) == PASS) {
        //log_info("Primary FW Rev = %s ", ver.Version_FW);
        strcpy((char *)ShmPrimaryMcuData->version, ver.Version_FW);
        strcpy((char *) pSysInfo->CsuPrimFwRev, ver.Version_FW);
        return TRUE;
    }
    return FALSE;
    //if (Query_HW_Ver(fd, OP_ADDR_IO_EXTEND, &ver) == PASS)
    //    ;//log_info("Primary HW Rev  = %s ", ver.Version_HW);
}

void GetInputGpioStatus(int fd)
{
    uint8_t dispenserSwTmp = 0;
    Gpio_in gpio_in = {0};
    static uint8_t dispenserSw = 0;

    //log_info("GetInputGpioStatus ");
    if (Query_Gpio_Input(fd, OP_ADDR_IO_EXTEND, &gpio_in) != PASS) {
        return;
    }

    ShmPrimaryMcuData->InputDet.bits.SpdDetec = gpio_in.SPD;
    //log_info("gpio_in.AC_Drop:%d", gpio_in.AC_Drop);
    if ((strncmp((char *)&pSysConfig->ModelName[7], "V", 1) == 0) ||
        (strncmp((char *)&pSysConfig->ModelName[9], "V", 1) == 0) ||
        (strncmp((char *)&pSysConfig->ModelName[7], "F", 1) == 0) ||
        (strncmp((char *)&pSysConfig->ModelName[9], "F", 1) == 0)
    ) {
        ShmPrimaryMcuData->InputDet.bits.Ac_Drop = gpio_in.AC_Drop; // Chiller Alarm ping
    }
#ifdef DD360ComBox
    EmgBtn_flag = 0;
#else
    if (gpio_in.Emergency_Btn && (EmgBtn_flag != gpio_in.Emergency_Btn))
    {
            EmgBtn_count++;
        if (EmgBtn_count > SensorTrigCount) {
           EmgBtn_flag = 1;
           EmgBtn_count = 0; // Avoid Overflow
       }
    } else if (EmgBtn_flag != gpio_in.Emergency_Btn ) {
        EmgBtn_count++;
        if (EmgBtn_count > SensorTrigCount) {
            EmgBtn_flag = 0;
            EmgBtn_count = 0;
        }
    }
#endif
    ShmPrimaryMcuData->InputDet.bits.EmergencyButton = EmgBtn_flag;

    dispenserSwTmp |= (ShmPrimaryMcuData->InputDet.bits.Key0);
    dispenserSwTmp |= (ShmPrimaryMcuData->InputDet.bits.Key1 << 1);
    dispenserSwTmp |= (ShmPrimaryMcuData->InputDet.bits.Key2 << 2);
    dispenserSwTmp |= (ShmPrimaryMcuData->InputDet.bits.Key3 << 3);

    if (dispenserSwTmp != dispenserSw) {
        dispenserSw = dispenserSwTmp;
        log_info("Dispenser switch number =  %d, bit = %d, %d, %d, %d",
                 dispenserSw,
                 ShmPrimaryMcuData->InputDet.bits.Key3,
                 ShmPrimaryMcuData->InputDet.bits.Key2,
                 ShmPrimaryMcuData->InputDet.bits.Key1,
                 ShmPrimaryMcuData->InputDet.bits.Key0);
    }

    ShmPrimaryMcuData->InputDet.bits.Button1 = gpio_in.Button[0];
    ShmPrimaryMcuData->InputDet.bits.Button2 = gpio_in.Button[1];

//#if defined DD360 || defined DD360Audi || defined DD360ComBox
    if ((strncmp((char *)&pSysConfig->ModelName[7], "V", 1) == 0) ||
            (strncmp((char *)&pSysConfig->ModelName[9], "V", 1) == 0) ||
            (strncmp((char *)&pSysConfig->ModelName[7], "F", 1) == 0) ||
            (strncmp((char *)&pSysConfig->ModelName[9], "F", 1) == 0)
       ) {
        pAlarmCode->AlarmEvents.bits.CcsLiquidChillerWaterLevelWarning = ~gpio_in.AC_Connector;
        pFaultCode->FaultEvents.bits.CcsLiquidChillerWaterLevelFault = ~gpio_in.AC_MainBreaker;
    } else {
        //pAlarmCode->AlarmEvents.bits.CcsLiquidChillerWaterLevelWarning = gpio_in.AC_Connector;
        //pFaultCode->FaultEvents.bits.CcsLiquidChillerWaterLevelFault = gpio_in.AC_MainBreaker;
    }

#if defined DD360ComBox
    Door_flag = 0;
#else
    if (Door_flag == gpio_in.Door_Open) {
        Door_count++;
        if (Door_count == 3 ) {
            Door_count = 0;
            Door_flag = gpio_in.Door_Open;
        }
    } else { 
        Door_flag = gpio_in.Door_Open;
        Door_count = 0;
    }
    /*
    if (gpio_in.Door_Open == 0 && (Door_flag == gpio_in.Door_Open))
    {
        Door_count++;
        if (Door_count > SensorTrigCount) {
            Door_flag = 1;
            Door_count = 0; // Avoid Overflow
       }
    } else if (gpio_in.Door_Open && Door_flag) {
        Door_count++;
        if (Door_count > SensorTrigCount) {
            Door_flag = 0;
            Door_count = 0;
        }
    }
    */
#endif 
#if defined DD360ComBox
    ShmPrimaryMcuData->InputDet.bits.DoorOpen = Door_flag;
#else
    ShmPrimaryMcuData->InputDet.bits.DoorOpen = ~Door_flag;
#endif

/*
    log_info("Emergency Button Count = %d , Emergency flag = %d",
            EmgBtn_count,EmgBtn_flag);
    log_info("Door Sensor Count = %d , Door Sensor flag = %d",
            Door_count,Door_flag);
*/

    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;

    return;


}

static void checkChillerStatus(Gpio_out *gpio)
{
    uint8_t gunIndex = 0;
    uint8_t chillerCount = 0;
    struct ChargingInfoData *pDcChargingInfo = NULL;
    static ChillerInfo fChillerInfo[2] = {0}, *pChillerInfo = NULL;
    static ChillerInfo _chiller;
    struct FanModuleData* ShmFanModuleData = (struct FanModuleData*)GetShmFanModuleData();
    DcCommonInfo* ShmDcCommonData = (DcCommonInfo*)GetShmDcCommonData();
    Gpio_out *pGpio = (Gpio_out *)gpio;

    if ((strncmp((char*)&pSysConfig->ModelName[7], "V", 1) == 0) ||
        (strncmp((char*)&pSysConfig->ModelName[7], "F", 1) == 0)) {
        chillerCount++;
        ShmDcCommonData->pGunInfo[0].withChiller = TRUE;
    }

    if ((strncmp((char*)&pSysConfig->ModelName[9], "V", 1) == 0) ||
        (strncmp((char*)&pSysConfig->ModelName[9], "F", 1) == 0)) {
        chillerCount++;
        ShmDcCommonData->pGunInfo[1].withChiller = TRUE;
    }

    if (chillerCount == 0) {
        pGpio->AC_Connector = 0x00;
        return;
    }
    // 設定chiller 開關
    for (gunIndex = 0; gunIndex < pSysConfig->TotalConnectorCount; gunIndex++) {
        if (!ShmDcCommonData->pGunInfo[gunIndex].withChiller)
            continue;
        pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(gunIndex);
        pChillerInfo = (ChillerInfo *)&fChillerInfo[gunIndex];

        if((pDcChargingInfo->SystemStatus > S_IDLE && pDcChargingInfo->SystemStatus < S_TERMINATING) ||
            (pDcChargingInfo->SystemStatus >= S_CCS_PRECHARGE_ST0 && pDcChargingInfo->SystemStatus <= S_CCS_PRECHARGE_ST1)) {
            pChillerInfo->ChillerSwitch = YES;
            pChillerInfo->ChillerOnTime = time((time_t *)NULL);
        } else {
            if(pChillerInfo->ChillerSwitch == YES) {
                //10分鐘後停止
                if ((time((time_t *)NULL) - pChillerInfo->ChillerOnTime) >= 600) {
                    pChillerInfo->ChillerSwitch = NO;
                    ShmDcCommonData->FanOnTime = time((time_t*)NULL);
                }
            } else {
                pChillerInfo->ChillerSwitch = NO;
                ShmFanModuleData->SetFan1Speed = 0;
            }
        }
        // 檢查Chiller溫度點，若小於零下時開啟heater，大於10度時關閉heater
        if (pDcChargingInfo->ChillerTemp < 70) {
            pGpio->AC_Breaker = YES;
        } else if(pDcChargingInfo->ChillerTemp > 75) {
            pGpio->AC_Breaker = NO;
        }
        //log_info("Gun%d Set Heater %s", gunIndex, pGpio->AC_Breaker ? "ON" : "OFF");
    }
    uint8_t _chillerNeedOn = NO;
    for (gunIndex = 0; gunIndex < pSysConfig->TotalConnectorCount; gunIndex++)
    {
        pChillerInfo = (ChillerInfo*)&fChillerInfo[gunIndex];
        if (pChillerInfo->ChillerSwitch == YES) {
            _chillerNeedOn = YES;
            ShmFanModuleData->SetFan1Speed = 7000;
        }
    }
    /*
    if (ShmDcCommonData->debugflag == YES)
        _chillerNeedOn = ShmDcCommonData->chillerCtrl;
    */
    if (ShmPrimaryMcuData->InputDet.bits.Ac_Drop == ABNORMAL) {
        _chillerNeedOn = NO;
    }

    if(_chiller.ChillerSwitch != _chillerNeedOn) {
        log_info("Chiller Need Turn %s", _chillerNeedOn == YES ? "ON" : "OFF");
    }
    _chiller.ChillerSwitch = _chillerNeedOn;
    pGpio->AC_Connector = _chiller.ChillerSwitch;//Chiller ON/OFF Control, "0: Chiller disable, 1: Chiller enable"

}

void SetOutputGpio(int fd, uint8_t outputValue)
{
    Gpio_out gpio;
    LedConfig *pLedConfig = (LedConfig *)&outputValue;
    static uint8_t flash = NO;

    if (strcmp((char *)pSysInfo->LcmHwRev, " ") == 0x00) {
        if (flash == NO) {
            flash = YES;
        } else {
            flash = NO;
        }
    } else {
        if (flash == NO) {
            flash = YES;
        }
    }

    pLedConfig->LeftButtonLed = flash;
    pLedConfig->RightButtonLed = flash;

    gpio.Button_LED[0] = pLedConfig->LeftButtonLed;
    gpio.Button_LED[1] = pLedConfig->RightButtonLed;
#if defined DD360ComBox
    gpio.System_LED[0] = pLedConfig->GreenLED;
    gpio.System_LED[1] = pLedConfig->YellowLED;
    gpio.System_LED[2] = pLedConfig->RedLED;
    gpio.System_LED[3] = 0x00;
#else
    if (_isNeedChkTilt) {
        gpio.System_LED[0] = 0x00;
        gpio.System_LED[1] = 0x00;
        gpio.System_LED[2] = tilt;
        gpio.System_LED[3] = tilt;
    }
#endif
    checkChillerStatus(&gpio);

    //gpio.AC_Breaker = 0x01;

    Config_Gpio_Output(fd, OP_ADDR_IO_EXTEND, &gpio);
}

void SetRtcData(int fd)
{
    Rtc rtc = {0};
    struct timeb csuTime;
    struct tm *tmCSU;

    ftime(&csuTime);
    tmCSU = localtime(&csuTime.time);
    //log_info("Time : %04d-%02d-%02d %02d:%02d:%02d ",
    //         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(fd, OP_ADDR_IO_EXTEND, &rtc) == PASS) {
        //log_info("SetRtc sucessfully. ");
    } else {
        //log_info("SetRtc fail. ");
    }
}

void SetModelName(int fd)
{
    if (Config_Model_Name(fd, OP_ADDR_IO_EXTEND, pSysConfig->ModelName) == PASS) {
    }
}

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

    fd = open(priPortName, O_RDWR);
    if (fd <= 0) {
#ifdef SystemLogMessage
        log_error("open 407 Communication port NG ");
#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] = (uint8_t)1;
    tios.c_lflag = 0;
    tcflush(fd, TCIFLUSH);
    ioctl (fd, TCSETS, &tios);

    return fd;
}

int GetTimeoutValue(struct timespec *startTime)
{
	struct timespec endTime;

	clock_gettime(CLOCK_MONOTONIC_COARSE, &endTime);
	return endTime.tv_sec - startTime->tv_sec;
}

void GetTimespecFunc(struct timespec *time)
{
	clock_gettime(CLOCK_MONOTONIC_COARSE, time);
}
static bool IsPrimaryProcessNeedPause(void)
{
    bool _pause = false;
    static bool isPause = false;
    struct ChargingInfoData *pDcChargingInfo = NULL;

    for (uint8_t i = 0; i < pSysConfig->TotalConnectorCount; i++)
    {
        pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(i);
        if(pDcChargingInfo->SystemStatus == S_UPDATE)
        {
            _pause = true;
        }
    }
    if(isPause != _pause)
    {
        log_info("Primary Process Now Is %s ", _pause == true ? "Paused" : "Continued");
    }
    isPause = _pause;

    return _pause;
}
bool CheckCustomer()
{
	bool result = false;
	char _buf[3] = {0};

	memcpy(_buf, &pSysConfig->ModelName[12], 2);
	if (strcmp(_buf, "OL") == EQUAL)
		result = true;
	return result;
}
void TiltSensorSwitch(bool _switch)
{
	if (tilt != _switch)
		tilt = _switch;

}
bool isChargingStatus(unsigned char status)
{
    if ((status >= S_REASSIGN_CHECK && status <= S_ALARM) ||
        status == S_CCS_PRECHARGE_ST0 || status == S_CCS_PRECHARGE_ST1 ) {
        return TRUE;
    } else {
        return FALSE;
    }
}
void ChkTiltSensor(fd)
{
    struct ChargingInfoData *pDcChargingInfo = NULL;
    bool isCharing = NO;
    Gpio_in gpio_in = {0};

    if (Query_Gpio_Input(fd, OP_ADDR_IO_EXTEND, &gpio_in) != PASS) {
        return;
    }
    for (uint8_t i = 0; i < pSysConfig->TotalConnectorCount; i++)
    {
        pDcChargingInfo = (struct ChargingInfoData *)GetDcChargingInfoData(i);
        if (isChargingStatus(pDcChargingInfo->SystemStatus) || ShmPrimaryMcuData->SelfTest_Comp != PASS) {
            isCharing = YES;
        }
    }

	if (isCharing == YES)
	{
		if (_chargeSwitch == NO)
		{
			if (ShmDcCommonData->_tiltSensorStep == _TILT_SENSOR_STEP_NONE)
			{
				log_info("---- Tilt Sensor : _TILT_SENSOR_STEP_PIN_ON ---- ");
				ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_PIN_ON;
				_chargeSwitch = YES;
			}
		}
		else
		{
			switch (ShmDcCommonData->_tiltSensorStep)
			{
				case _TILT_SENSOR_STEP_PIN_ON:
				{
					TiltSensorSwitch(YES);
					GetTimespecFunc(&_tilt_time);
					log_info("---- Tilt Sensor : _TILT_SENSOR_STEP_PIN_WAIT ---- ");
					ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_PIN_WAIT;
				}
				break;
				case _TILT_SENSOR_STEP_PIN_WAIT:
				{
					int _time = GetTimeoutValue(&_tilt_time);
					if (_time < 0)
						GetTimespecFunc(&_tilt_time);
					else if (_time > 1)
					{
						if (gpio_in.Door_Open == NO)
							pAlarmCode->AlarmEvents.bits.TiltSensorStestFail = YES;
						else
							pAlarmCode->AlarmEvents.bits.TiltSensorStestFail = NO;

						if (_time > TILTSENSORDET)
						{
							GetTimespecFunc(&_tilt_time);
							log_info("---- Tilt Sensor : _TILT_SENSOR_STEP_PIN_OFF ---- ");
                            log_info("---- Tilt Sensor : _TILT_SENSOR_TEST %s ---- ",
                                pAlarmCode->AlarmEvents.bits.TiltSensorStestFail ? "FAIL" : "PASS");
							ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_PIN_OFF;
						}
					}
				}
                break;
				case _TILT_SENSOR_STEP_PIN_OFF:
				{
					TiltSensorSwitch(NO);
					GetTimespecFunc(&_tilt_time);
					log_info("---- Tilt Sensor : _TILT_SENSOR_STEP_PIN_FINISH ---- ");
					ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_PIN_FINISH;
				}
                break;
				case _TILT_SENSOR_STEP_PIN_FINISH :
				{
					int _time = GetTimeoutValue(&_tilt_time);
					if (_time < 0)
						GetTimespecFunc(&_tilt_time);
					else if (_time > TILTSENSORDET)
					{
						log_info("---- Tilt Sensor : _TILT_SENSOR_STEP_NONE ---- ");
						ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_NONE;
                        ShmPrimaryMcuData->SelfTest_Comp = PASS;
					}
				}
                break;
			}
		}
	}
	else
	{
		TiltSensorSwitch(NO);
		ShmDcCommonData->_tiltSensorStep = _TILT_SENSOR_STEP_NONE;
		_chargeSwitch = NO;
        //log_info("ShmPrimaryMcuData->SelfTest_Comp:%d",ShmPrimaryMcuData->SelfTest_Comp);
	}
    return;
}


int main(void)
{
    int Uart1Fd = -1;

    //if (InitShareMemory() == FAIL) {
    //    log_error("InitShareMemory NG");
    //    if (ShmStatusCodeData != NULL) {
    //        pAlarmCode->AlarmEvents.bits.FailToCreateShareMemory = 1;
    //    }
    //    sleep(5);
    //    return 0;
    //}

    if (CreateAllCsuShareMemory() == FAIL) {
        log_error("create share memory error");
        return FAIL;
    }

    MappingGunChargingInfo("Primary Task");

    pSysConfig = (struct SysConfigData *)GetShmSysConfigData();
    pSysInfo = (struct SysInfoData *)GetShmSysInfoData();
    pAlarmCode = (struct AlarmCodeData *)GetShmAlarmCodeData();
    pFaultCode = (struct FaultCodeData *)GetShmFaultCodeData();
    ShmPrimaryMcuData = (struct PrimaryMcuData *)GetShmPrimaryMcuData();
    ShmDcCommonData = (DcCommonInfo *)GetShmDcCommonData();

    Uart1Fd = InitComPort();
    //log_info("407 Port id = %d ", Uart1Fd);

    if (Uart1Fd < 0) {
        log_error("InitComPort (Uart1 : AM3352 - STM32) NG");

        if (pAlarmCode != NULL) {
            pAlarmCode->AlarmEvents.bits.CsuInitFailed = 1;
        }
        sleep(5);

        return 0;
    }

    SetRtcData(Uart1Fd);
    SetModelName(Uart1Fd);

    gun_count = pSysConfig->TotalConnectorCount;

    //Initialization();

    // 需要判斷這個的客戶碼 : OL
	//_isNeedChkTilt = CheckCustomer();

    for (;;) {
        // 程序開始之前~ 必須先確定 FW 版本與硬體版本，確認後!!~ 該模組才算是真正的 Initial Comp.
        // 模組更新 FW 後，需重新做
        if(IsPrimaryProcessNeedPause() == true)
        {
            sleep(1);
            continue;
        }
        if (ShmPrimaryMcuData->SelfTest_Comp != PASS) {
            //log_info("(407) Get Fw and Hw Ver. ");
            if (GetFwAndHwVersion(Uart1Fd) != PASS ) {
                sleep(1);
                continue;
            }
            if (_isNeedChkTilt) {
                ChkTiltSensor(Uart1Fd);
            } else {
                ShmPrimaryMcuData->SelfTest_Comp = PASS;
            }
        } else {
            if (_isNeedChkTilt) {
                ChkTiltSensor(Uart1Fd);
            }
            SetOutputGpio(Uart1Fd, ShmPrimaryMcuData->OutputDrv.OutputDrvValue[0]);

            GetInputGpioStatus(Uart1Fd);
        }
        usleep(50000);
    }

    return FAIL;
}