csu3_am335x/EVSE/Projects/DM30/Apps/Module_InternalComm.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	"internalComm.h"
#include 	<stdbool.h>

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

struct SysConfigAndInfo			*ShmSysConfigAndInfo;
struct StatusCodeData 			*ShmStatusCodeData;
struct FanModuleData			*ShmFanModuleData;
struct RelayModuleData			*ShmRelayModuleData;
struct CHAdeMOData				*ShmCHAdeMOData;
struct CcsData					*ShmCcsData;

#define VIN_MAX_VOLTAGE		250	// 大於該值 : OVP
#define VIN_MIN_VOLTAGE		170	// 小於該值 : UVP
#define VIN_DROP_VOLTAGE	150	// 小於該值 : ac drop

#define VOUT_MAX_VOLTAGE	750
#define VOUT_MIN_VOLTAGE	150
#define IOUT_MAX_CURRENT	50

#define MAX_FAN_SPEED		6000
#define MIN_FAN_SPEED		300

//Warning : 150, PreWarning : 500
#define GFD_VALUE			500

// 最小切換 Relay 電壓
#define SELF_TO_CHANGE_RELAY_STATUS			600
// 透過電壓確認 Relay 是否搭上的依據電壓
#define CHECK_RELAY_STATUS					300
#define CHECK_RELAY_STATUS_GAP				100
// 安全在停止充電程序中斷開 Relay 的電流
#define SEFETY_SWITCH_RELAY_CUR				20
// 確認 Relay Welding 電壓
#define RELAY_WELDING_DET					300

byte gunCount = CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY;
// 槍資訊
struct ChargingInfoData *_chargingData[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY];
byte gfdChkFailCount[CHAdeMO_QUANTITY + CCS_QUANTITY + GB_QUANTITY];

bool FindChargingInfoData(byte target, struct ChargingInfoData **chargingData);

int Uart5Fd;
char *relayRs485PortName = "/dev/ttyS5";
unsigned short fanSpeedSmoothValue = 100;

struct timeval _priority_time;

Ver ver;
PresentInputVoltage inputVoltage;
PresentOutputVoltage outputVoltage;
FanSpeed fanSpeed;
Temperature temperature;
AuxPower auxPower;
Gfd gfd_adc;
Gfd_config gfd_config;
Gpio_in gpio_in;
Gpio_out gpio_out;
Relay outputRelay;
Relay regRelay;

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)
{
	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;
}

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

unsigned short MaxValue(unsigned short value1, unsigned short value2)
{
	return value1 >= value2 ? value1 : value2;
}

//==========================================
// Communication Function
//==========================================
void GetFwAndHwVersion_Aux()
{
	if (Query_FW_Ver(Uart5Fd, Addr.Aux, &ver) == PASS)
	{
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.AuxPwrFwRev, ver.Version_FW);
		printf("s1 = %s \n", ver.Version_FW);
	}

	if (Query_HW_Ver(Uart5Fd, Addr.Aux, &ver) == PASS)
	{
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.AuxPwrHwRev, ver.Version_HW);
		printf("s2 = %s \n", ver.Version_HW);
	}
}

void GetFwAndHwVersion_Fan()
{
	if(Query_FW_Ver(Uart5Fd, Addr.Fan, &ver) == PASS)
	{
		// FanModuleData
		strcpy((char *) ShmFanModuleData->version, ver.Version_FW);
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.FanModuleFwRev, ver.Version_FW);
		printf("GetFwAndHwVersion_Fan s1 = %s \n", ver.Version_FW);
	}

	if (Query_HW_Ver(Uart5Fd, Addr.Fan, &ver) == PASS)
	{
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.FanModuleHwRev, ver.Version_FW);
		printf("GetFwAndHwVersion_Fan s2 = %s \n", ver.Version_HW);
	}
}

void GetFwAndHwVersion_Relay()
{
	if (Query_FW_Ver(Uart5Fd, Addr.Relay, &ver) == PASS)
	{
		// FanModuleData
		strcpy((char *) ShmRelayModuleData->version, ver.Version_FW);
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.RelayModuleFwRev, ver.Version_FW);
		printf("GetFwAndHwVersion_Relay s1 = %s \n", ver.Version_FW);
	}

	if (Query_HW_Ver(Uart5Fd, Addr.Relay, &ver) == PASS)
	{
		// SystemInfo
		strcpy((char *) ShmSysConfigAndInfo->SysInfo.RelayModuleHwRev, ver.Version_FW);
		printf("GetFwAndHwVersion_Relay s2 = %s \n", ver.Version_HW);
	}
}

void GetTemperature_Aux()
{
	memset(temperature.temperature, 0, ARRAY_SIZE(temperature.temperature));
	if (Query_Temperature(Uart5Fd, Addr.Aux, &temperature) == PASS)
	{
		// aux temp
		// UI 實際顯示溫度轉換
		//char s[4];
		//sprintf(s,"%d",(-60 + temperature[2].temperature[4])),
		//printf("s = %s \n",s);

		printf("Aux temp = %d,%d,%d,%d,%d,%d,%d,%d \n",
				(-60 + temperature.temperature[0]),
				(-60 + temperature.temperature[1]),
				(-60 + temperature.temperature[2]),
				(-60 + temperature.temperature[3]),
				(-60 + temperature.temperature[4]),
				(-60 + temperature.temperature[5]),
				(-60 + temperature.temperature[6]),
				(-60 + temperature.temperature[7]));
	}
}

void GetTemperature_Relay()
{
	memset(temperature.temperature, 0, ARRAY_SIZE(temperature.temperature));
	if (Query_Temperature(Uart5Fd, Addr.Relay, &temperature) == PASS)
	{
		// relay temp
		printf("Relay temp = %d,%d,%d,%d,%d,%d,%d,%d \n",
				(-60 + temperature.temperature[0]),
				(-60 + temperature.temperature[1]),
				(-60 + temperature.temperature[2]),
				(-60 + temperature.temperature[3]),
				(-60 + temperature.temperature[4]),
				(-60 + temperature.temperature[5]),
				(-60 + temperature.temperature[6]),
				(-60 + temperature.temperature[7]));
	}
}

// AC 三相輸入電壓
void GetPresentInputVol()
{
	if (Query_Present_InputVoltage(Uart5Fd, Addr.Relay, &inputVoltage) == PASS)
	{
		// resolution : 0.1
		//printf("InputVoltageR = %f \n", inputVoltage.L1N_L12);
		//printf("InputVoltageS = %f \n", inputVoltage.L2N_L23);
		//printf("InputVoltageT = %f \n", inputVoltage.L3N_L31);

		ShmRelayModuleData->InputL1Volt = inputVoltage.L1N_L12;
		ShmRelayModuleData->InputL2Volt = inputVoltage.L2N_L23;
		ShmRelayModuleData->InputL3Volt = inputVoltage.L3N_L31;

		//********************************************************************************************************//
		// VIN < 170
		if (inputVoltage.L1N_L12 < VIN_MIN_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputUVP = 0x01;
		}
		if (inputVoltage.L2N_L23 < VIN_MIN_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputUVP = 0x01;
		}
		if (inputVoltage.L3N_L31 < VIN_MIN_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputUVP = 0x01;
		}

		//********************************************************************************************************//
		// VIN > 250
		if (inputVoltage.L1N_L12 > VIN_MAX_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputOVP = 0x01;
		}
		if (inputVoltage.L2N_L23 > VIN_MAX_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputOVP = 0x01;
		}
		if (inputVoltage.L3N_L31 > VIN_MAX_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputOVP = 0x01;
		}
		//********************************************************************************************************//
		// VIN < 150
		if (inputVoltage.L1N_L12 < VIN_DROP_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputDrop = 0x01;
		}
		if (inputVoltage.L2N_L23 < VIN_DROP_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputDrop = 0x01;
		}
		if (inputVoltage.L3N_L31 < VIN_DROP_VOLTAGE)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputDrop = 0x01;
		}

		//********************************************************************************************************//
		// 150 <= VIN < 160
//		if (inputVoltage.L1N_L12 >= VIN_MIN_VOLTAGE && inputVoltage.L1N_L12 <= VIN_LOW_VOLTAGE)
//		{
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputUVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputOVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL1InputDrop = 0x00;
//		}
//		if (inputVoltage.L2N_L23 >= VIN_MIN_VOLTAGE && inputVoltage.L2N_L23 <= VIN_LOW_VOLTAGE)
//		{
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputUVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputOVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL2InputDrop = 0x00;
//		}
//		if (inputVoltage.L3N_L31 >= VIN_MIN_VOLTAGE && inputVoltage.L3N_L31 <= VIN_LOW_VOLTAGE)
//		{
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputUVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputOVP = 0x00;
//			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemL3InputDrop = 0x00;
//		}
	}
}

// 左右槍的 Relay 前後的輸出電壓
void GetPersentOutputVol()
{
	if (Query_Present_OutputVoltage(Uart5Fd, Addr.Relay, &outputVoltage) == PASS)
	{
//		printf("Conn1 fuse 1 = %f \n", outputVoltage.behindFuse_Voltage_C1);
//		printf("Conn1 relay 1 = %f \n", outputVoltage.behindRelay_Voltage_C1);
//		printf("Conn2 fuse 2 = %f \n", outputVoltage.behindFuse_Voltage_C2);
//		printf("Conn2 relay 2 = %f \n", outputVoltage.behindRelay_Voltage_C2);

		//printf("outputVoltage.behindFuse_Voltage_C1 = %f \n", outputVoltage.behindFuse_Voltage_C1);
		//printf("outputVoltage.behindFuse_Voltage_C2 = %f \n", outputVoltage.behindFuse_Voltage_C2);

		ShmRelayModuleData->Gun1FuseOutputVolt = outputVoltage.behindFuse_Voltage_C1;
		ShmRelayModuleData->Gun1RelayOutputVolt = outputVoltage.behindRelay_Voltage_C1;
		ShmRelayModuleData->Gun2FuseOutputVolt = outputVoltage.behindFuse_Voltage_C2;
		ShmRelayModuleData->Gun2RelayOutputVolt = outputVoltage.behindRelay_Voltage_C2;

		for (int index = 0; index < gunCount; index++)
		{
			if (index == 0)
			{
				if (_chargingData[index]->Evboard_id == 0x01)
				{
					_chargingData[index]->FuseChargingVoltage = ShmRelayModuleData->Gun1FuseOutputVolt;
					_chargingData[index]->FireChargingVoltage = ShmRelayModuleData->Gun1RelayOutputVolt;
				}
				else if (_chargingData[index]->Evboard_id == 0x02)
				{
					_chargingData[index]->FuseChargingVoltage = ShmRelayModuleData->Gun2FuseOutputVolt;
					_chargingData[index]->FireChargingVoltage = ShmRelayModuleData->Gun2RelayOutputVolt;
				}
			}
			else if (index == 1)
			{
				_chargingData[index]->FuseChargingVoltage = ShmRelayModuleData->Gun2FuseOutputVolt;
				_chargingData[index]->FireChargingVoltage = ShmRelayModuleData->Gun2RelayOutputVolt;
			}

			unsigned short Ovp = 0;
			unsigned short Ocp = 0;
			//Ovp = MIN [VOUT_MAX_VOLTAGE, EV_BATTERY_VOLTAGE] 	// 最大輸出電壓與電池電壓最大值
			//Ocp = MIN [IOUT_MAX_CURRENT, EV_CURRENT_REQ]		// 最大輸出電流與需求電流最小值
			if (_chargingData[index]->Type == _Type_Chademo)
			{
				Ovp = MaxValue(_chargingData[index]->MaximumChargingVoltage, _chargingData[index]->EvBatteryMaxVoltage);
				Ocp = MaxValue(_chargingData[index]->PresentChargingCurrent, ShmCHAdeMOData->ev[_chargingData[index]->type_index].ChargingCurrentRequest);
				if (_chargingData[index]->PresentChargingVoltage >= Ovp)
				{
					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemChademoOutputOVP = 0x01;
				}
				if (_chargingData[index]->PresentChargingCurrent >= Ocp)
				{
					ShmStatusCodeData->AlarmCode.AlarmEvents.bits.SystemChademoOutputOCP = 0x01;
				}
			}
			else if (_chargingData[index]->Type == _Type_CCS)
			{

			}
		}
	}
}

// 風扇速度
void GetFanSpeed()
{
	//printf("Get fan board speed \n");
	if (Query_Fan_Speed(Uart5Fd, Addr.Fan, &fanSpeed) == PASS)
	{
		ShmFanModuleData->PresentFan1Speed = fanSpeed.speed[0];
		ShmFanModuleData->PresentFan2Speed = fanSpeed.speed[1];
		ShmFanModuleData->PresentFan3Speed = fanSpeed.speed[2];
		ShmFanModuleData->PresentFan4Speed = fanSpeed.speed[3];
		printf("SystemFanRotaSpeed_1 = %d \n", fanSpeed.speed[0]);
		printf("SystemFanRotaSpeed_2 = %d \n", fanSpeed.speed[1]);
		printf("SystemFanRotaSpeed_3 = %d \n", fanSpeed.speed[2]);
		printf("SystemFanRotaSpeed_4 = %d \n", fanSpeed.speed[3]);
		// Config_Fan_Speed(Uart5Fd, Addr.Fan, &fanSpeed[0]);
		//SysInfoData (SystemFanRotaSpeed)
	}
}

// 讀取 Relay 狀態
void GetRelayOutputStatus()
{
	if (Query_Relay_Output(Uart5Fd, Addr.Relay, &regRelay) == PASS)
	{
		regRelay.relay_event.bits.AC_Contactor = ShmSysConfigAndInfo->SysInfo.AcContactorStatus;
	}
}

// 確認 K1 K2 relay 的狀態
void CheckK1K2RelayOutput(byte index)
{
	if (index == 0)
	{
		if (_chargingData[index]->Evboard_id == 0x01)
		{
			if (regRelay.relay_event.bits.Gun1_N == YES && regRelay.relay_event.bits.Gun1_P == YES)
				_chargingData[index]->RelayK1K2Status = YES;
			else
				_chargingData[index]->RelayK1K2Status = NO;

			if(_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.Gun1_N == YES	&& regRelay.relay_event.bits.CCS_Precharge == YES)
					_chargingData[index]->RelayKPK2Status = YES;
				else
					_chargingData[index]->RelayKPK2Status = NO;
			}
		}
		else if (_chargingData[index]->Evboard_id == 0x02)
		{
			if (regRelay.relay_event.bits.Gun2_N == YES && regRelay.relay_event.bits.Gun2_P == YES)
				_chargingData[index]->RelayK1K2Status = YES;
			else
				_chargingData[index]->RelayK1K2Status = NO;

			if(_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.Gun2_N == YES	&& regRelay.relay_event.bits.CCS_Precharge == YES)
					_chargingData[index]->RelayKPK2Status = YES;
				else
					_chargingData[index]->RelayKPK2Status = NO;
			}

		}
	}
	else if (index == 1)
	{
		if (regRelay.relay_event.bits.Gun2_N == YES && regRelay.relay_event.bits.Gun2_P == YES)
			_chargingData[index]->RelayK1K2Status = YES;
		else
			_chargingData[index]->RelayK1K2Status = NO;

		if(_chargingData[index]->Type == _Type_CCS)
		{
			if (regRelay.relay_event.bits.Gun2_N == YES && regRelay.relay_event.bits.CCS_Precharge == YES)
				_chargingData[index]->RelayKPK2Status = YES;
			else
				_chargingData[index]->RelayKPK2Status = NO;
		}
	}

	if (regRelay.relay_event.bits.Gun1_Parallel_N == YES && regRelay.relay_event.bits.Gun1_Parallel_P == YES)
		ShmSysConfigAndInfo->SysInfo.BridgeRelayStatus = YES;
	else
		ShmSysConfigAndInfo->SysInfo.BridgeRelayStatus = NO;
}

void GetGfdAdc()
{
	// define : 每 0.2 ~ 1 秒一次
	// occur : <= 75k 歐姆 @ 150 - 750 Vdc
	// warning : >= 100 歐姆 && <= 500 歐姆 @ 150-750 Vdc
	if (Query_Gfd_Adc(Uart5Fd, Addr.Relay, &gfd_adc) == PASS)
	{
		for (int i = 0; i < gunCount; i++)
		{
			if (i == 0)
			{
				_chargingData[i]->GroundFaultStatus = gfd_adc.result_conn1;
				if (_chargingData[i]->GroundFaultStatus == GFD_FAIL)
				{
					DEBUG_ERROR("GFD Fail. index = %d, R = %d, Vol = %d \n",
							i, gfd_adc.Resister_conn1, gfd_adc.voltage_conn1);
				}
			}
			else if (i == 1)
			{
				_chargingData[i]->GroundFaultStatus = gfd_adc.result_conn2;
				if (_chargingData[i]->GroundFaultStatus == GFD_FAIL)
				{
					DEBUG_ERROR("GFD Fail. index = %d, R = %d, Vol = %d \n",
							i, gfd_adc.Resister_conn2, gfd_adc.voltage_conn2);
				}
			}
		}

		//if (gfd_adc.result_conn1 != 0)
		{
			printf("******************Resister_conn1 = %d, voltage_conn1 = %d, result_conn1 = %d, step = %d \n",
				gfd_adc.Resister_conn1,
				gfd_adc.voltage_conn1,
				gfd_adc.result_conn1,
				gfd_adc.rb_step_1);
		}
	}
}

void GetGpioInput()
{
	if (Query_Gpio_Input(Uart5Fd, Addr.Aux, &gpio_in) == PASS)
	{
		// AC Contactor Status
		//ShmSysConfigAndInfo->SysInfo.AcContactorStatus = gpio_in.AC_Connector;

		if (gpio_in.AC_MainBreaker == 1)
		{
			// AC Main Breaker ON
			printf("RB AC Main Breaker. \n");
		}

		if (gpio_in.SPD == 1)
		{
			// SPD (雷擊保護) ON
			printf("RB SPD. \n");
		}

		if (gpio_in.Door_Open == 1)
		{
			// Door Open
			printf("RB Door Open. \n");
		}

		if (gpio_in.GFD[0] == 1)
		{
			// GFD_1 Trigger
		}

		if (gpio_in.GFD[1] == 1)
		{
			// GFD_2 Trigger
		}

		if (gpio_in.AC_Drop == 1)
		{
			// AC Drop
			printf("RB AC Drop. \n");
		}

		if (gpio_in.Emergency_IO == 1)
		{
			// Emergency IO ON
			printf("RB Emergency IO ON. \n");
		}

		if (gpio_in.Button_Emergency_Press == 1)
		{
			// Emergency button Press
		}

		if (gpio_in.Button_On_Press == 1)
		{
			// On button Press
		}

		if (gpio_in.Button_Off_Press == 1)
		{
			// Off button Press
		}

		if (gpio_in.Key_1_Press == 1)
		{
			// key 1 press
		}

		if (gpio_in.Key_2_Press == 1)
		{
			// key 2 press
		}

		if (gpio_in.Key_3_Press == 1)
		{
			// key 3 press
		}

		if (gpio_in.Key_4_Press == 1)
		{
			// key 4 press
		}
	}
}

// 5V 12V 24V 48V
void GetAuxPower()
{
	if (Query_Aux_PowerVoltage(Uart5Fd, Addr.Fan, &auxPower) == PASS)
	{
		ShmSysConfigAndInfo->SysInfo.AuxPower48V = auxPower.voltage[0];
		ShmSysConfigAndInfo->SysInfo.AuxPower24V = auxPower.voltage[1];
		//ShmSysConfigAndInfo->SysInfo.AuxPower12V = auxPower.voltage[4];
		//ShmSysConfigAndInfo->SysInfo.AuxPower5V = auxPower.voltage[6];
		// aux power voltage
		//printf("aux1 = %x, \n", auxPower.voltage[0]);
		//printf("aux2 = %x, \n", auxPower.voltage[1]);
	}
}

void SetFanModuleSpeed()
{
	// 調整風扇速度要漸進式 : 500 rpm/p
	if (ShmFanModuleData->PresentFan1Speed != ShmFanModuleData->SetFan1Speed ||
			ShmFanModuleData->PresentFan2Speed != ShmFanModuleData->SetFan2Speed ||
			ShmFanModuleData->PresentFan3Speed != ShmFanModuleData->SetFan3Speed ||
			ShmFanModuleData->PresentFan4Speed != ShmFanModuleData->SetFan4Speed)
	{
		FanSpeed _fanSpeed;

		unsigned short speed = ShmFanModuleData->PresentFan1Speed + fanSpeedSmoothValue;
		if (speed >= ShmFanModuleData->SetFan1Speed)
			speed = ShmFanModuleData->SetFan1Speed;
		_fanSpeed.speed[0] = speed & 0xff;
		_fanSpeed.speed[1] = (speed >> 8) & 0xff;

		speed = ShmFanModuleData->PresentFan2Speed + fanSpeedSmoothValue;
		if (speed >= ShmFanModuleData->SetFan2Speed)
			speed = ShmFanModuleData->SetFan2Speed;
		_fanSpeed.speed[2] = speed & 0xff;
		_fanSpeed.speed[3] = (speed >> 8) & 0xff;

		speed = ShmFanModuleData->PresentFan3Speed + fanSpeedSmoothValue;
		if (speed >= ShmFanModuleData->SetFan3Speed)
			speed = ShmFanModuleData->SetFan3Speed;
		_fanSpeed.speed[4] = speed & 0xff;
		_fanSpeed.speed[5] = (speed >> 8) & 0xff;

		speed = ShmFanModuleData->PresentFan4Speed + fanSpeedSmoothValue;
		if (speed >= ShmFanModuleData->SetFan4Speed)
			speed = ShmFanModuleData->SetFan4Speed;
		_fanSpeed.speed[6] = speed & 0xff;
		_fanSpeed.speed[7] = (speed >> 8) & 0xff;

		if (Config_Fan_Speed(Uart5Fd, Addr.Fan, &_fanSpeed) == PASS)
		{
			printf("successfully Fan\n");
		}
		else
			printf("fail Fan\n");
	}
}

void SetRelayModuleFanSpeed()
{
	// 調整風扇速度要漸進式 : 100 rpm/p
	if (ShmFanModuleData->PresentFan1Speed != ShmFanModuleData->SetFan1Speed)
	{
		FanSpeed _fanSpeed;
		unsigned short speed = 0;

		if (ShmFanModuleData->SetFan1Speed > ShmFanModuleData->PresentFan1Speed)
		{
			speed = ShmFanModuleData->PresentFan1Speed + fanSpeedSmoothValue;
			if (speed >= ShmFanModuleData->SetFan1Speed)
				speed = ShmFanModuleData->SetFan1Speed;
		}
		else
		{
			speed = ShmFanModuleData->PresentFan1Speed - fanSpeedSmoothValue;
			if (speed <= 0)
				speed = ShmFanModuleData->SetFan1Speed;
		}

		_fanSpeed.speed[0] = speed & 0xff;
		_fanSpeed.speed[1] = (speed >> 8) & 0xff;
		ShmFanModuleData->PresentFan1Speed = speed;

		Config_Fan_Speed(Uart5Fd, Addr.Relay, &_fanSpeed);
	}
}

void GetRelayModuleFanSpeed()
{
	printf("Get fan board speed \n");
	if (Query_Fan_Speed(Uart5Fd, Addr.Relay, &fanSpeed) == PASS)
	{
		ShmFanModuleData->PresentFan1Speed = fanSpeed.speed[0] + (fanSpeed.speed[1] >> 8);
		printf("SystemFanRotaSpeed_1 = %d \n", fanSpeed.speed[0]);
	}
}

//==========================================
// Common Function
//==========================================
void SetK1K2RelayStatus(byte index)
{
	if (_chargingData[index]->SystemStatus < S_PREPARING_FOR_EVSE)
	{
		if (_chargingData[index]->Evboard_id == 0x01)
		{
			if(regRelay.relay_event.bits.Gun1_P == YES)
				outputRelay.relay_event.bits.Gun1_P = NO;
			else if (regRelay.relay_event.bits.Gun1_N == YES)
				outputRelay.relay_event.bits.Gun1_N = NO;

			if (_chargingData[index]->Type == _Type_CCS)
			{
				if(regRelay.relay_event.bits.CCS_Precharge == YES)
					outputRelay.relay_event.bits.CCS_Precharge = NO;
			}
		}
		else if (_chargingData[index]->Evboard_id == 0x02)
		{
			if(regRelay.relay_event.bits.Gun2_P == YES)
				outputRelay.relay_event.bits.Gun2_P = NO;
			else if (regRelay.relay_event.bits.Gun2_N == YES)
				outputRelay.relay_event.bits.Gun2_N = NO;

			if (_chargingData[index]->Type == _Type_CCS)
			{
				if(regRelay.relay_event.bits.CCS_Precharge == YES)
					outputRelay.relay_event.bits.CCS_Precharge = NO;
			}
		}
	}
	else if ((_chargingData[index]->SystemStatus >= S_PREPARING_FOR_EVSE && _chargingData[index]->SystemStatus <= S_CHARGING))
	{
		if (_chargingData[index]->Evboard_id == 0x01)
		{
			if(regRelay.relay_event.bits.Gun1_N == NO)
				outputRelay.relay_event.bits.Gun1_N = YES;
			else if (regRelay.relay_event.bits.Gun1_P == NO)
				outputRelay.relay_event.bits.Gun1_P = YES;
		}
		else if (_chargingData[index]->Evboard_id == 0x02)
		{
			if(regRelay.relay_event.bits.Gun2_N == NO)
				outputRelay.relay_event.bits.Gun2_N = YES;
			else if (regRelay.relay_event.bits.Gun2_P == NO)
				outputRelay.relay_event.bits.Gun2_P = YES;
		}
	}
	else if (_chargingData[index]->SystemStatus == S_COMPLETE)
	{
		if (_chargingData[index]->PresentChargingCurrent <= SEFETY_SWITCH_RELAY_CUR)
		{
			if (_chargingData[index]->Evboard_id == 0x01)
			{
				if(regRelay.relay_event.bits.Gun1_P == YES)
					outputRelay.relay_event.bits.Gun1_P = NO;
				else if (regRelay.relay_event.bits.Gun1_N == YES)
					outputRelay.relay_event.bits.Gun1_N = NO;
			}
			else if (_chargingData[index]->Evboard_id == 0x02)
			{
				if(regRelay.relay_event.bits.Gun2_P == YES)
					outputRelay.relay_event.bits.Gun2_P = NO;
				else if (regRelay.relay_event.bits.Gun2_N == YES)
					outputRelay.relay_event.bits.Gun2_N = NO;
			}
		}
	}
	else if (_chargingData[index]->SystemStatus == S_CCS_PRECHARGE_ST0)
	{
		if (_chargingData[index]->Evboard_id == 0x01)
		{
			if (_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.CCS_Precharge == NO)
					outputRelay.relay_event.bits.CCS_Precharge = YES;
				else if (regRelay.relay_event.bits.CCS_Precharge == YES)
					outputRelay.relay_event.bits.Gun1_P = NO;
			}
		}
		else if (_chargingData[index]->Evboard_id == 0x02)
		{
			if (_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.CCS_Precharge == NO)
					outputRelay.relay_event.bits.CCS_Precharge = YES;
				else if (regRelay.relay_event.bits.CCS_Precharge == YES)
					outputRelay.relay_event.bits.Gun2_P = NO;
			}
		}
	}
	else if (_chargingData[index]->SystemStatus == S_CCS_PRECHARGE_ST1)
	{
		if (_chargingData[index]->Evboard_id == 0x01)
		{
			if (_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.Gun1_P == NO)
					outputRelay.relay_event.bits.Gun1_P = YES;
				else if(regRelay.relay_event.bits.Gun1_P == YES)
					outputRelay.relay_event.bits.CCS_Precharge = NO;
			}
		}
		else if (_chargingData[index]->Evboard_id == 0x02)
		{
			if (_chargingData[index]->Type == _Type_CCS)
			{
				if (regRelay.relay_event.bits.Gun2_P == NO)
					outputRelay.relay_event.bits.Gun2_P = YES;
				else if(regRelay.relay_event.bits.Gun2_P == YES)
					outputRelay.relay_event.bits.CCS_Precharge = NO;
			}
		}
	}
}

void SetParalleRelayStatus()
{
	if (gunCount >= 2)
	{
		if (_chargingData[0]->SystemStatus == S_BOOTING || _chargingData[1]->SystemStatus == S_BOOTING)
		{
			// 初始化~ 不搭橋接
			if (regRelay.relay_event.bits.Gun1_Parallel_P == YES)
				outputRelay.relay_event.bits.Gun1_Parallel_P = NO;
			else if (regRelay.relay_event.bits.Gun1_Parallel_N == YES)
				outputRelay.relay_event.bits.Gun1_Parallel_N = NO;
		}
		else
		{
			// 單槍充電中 - 搭上橋接
			if((_chargingData[0]->IsReadyToCharging == YES && _chargingData[1]->IsReadyToCharging == NO) ||
					(_chargingData[0]->IsReadyToCharging == NO && _chargingData[1]->IsReadyToCharging == YES))
			{
				if (ShmSysConfigAndInfo->SysInfo.ReAssignedFlag >= _REASSIGNED_RELAY)
				{
					if (regRelay.relay_event.bits.Gun1_Parallel_P == YES)
						outputRelay.relay_event.bits.Gun1_Parallel_P = NO;
					else if (regRelay.relay_event.bits.Gun1_Parallel_N == YES)
						outputRelay.relay_event.bits.Gun1_Parallel_N = NO;
				}
				else
				{
					if (regRelay.relay_event.bits.Gun1_Parallel_N == NO)
						outputRelay.relay_event.bits.Gun1_Parallel_N = YES;
					else if (regRelay.relay_event.bits.Gun1_Parallel_P == NO)
						outputRelay.relay_event.bits.Gun1_Parallel_P = YES;
				}
			}
			else
			{
				// 雙槍充電中~ 不搭橋接
				if (regRelay.relay_event.bits.Gun1_Parallel_P == YES)
					outputRelay.relay_event.bits.Gun1_Parallel_P = NO;
				else if (regRelay.relay_event.bits.Gun1_Parallel_N == YES)
					outputRelay.relay_event.bits.Gun1_Parallel_N = NO;
			}
		}
	}
}

//==========================================
// 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\n");
		#endif
		result = FAIL;
	}
	else if ((ShmSysConfigAndInfo = shmat(MeterSMId, NULL, 0)) == (void *) -1)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("[shmat ShmSysConfigAndInfo NG\n");
		#endif
		result = FAIL;
	 }
	 //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;
	}

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

	if(CHAdeMO_QUANTITY > 0)
	{
		if ((MeterSMId = shmget(ShmCHAdeMOCommKey, sizeof(struct CHAdeMOData),	IPC_CREAT | 0777)) < 0)
		{
			#ifdef SystemLogMessage
			DEBUG_ERROR("[shmget ShmCHAdeMOData NG \n");
			#endif
			return FAIL;
		}
		else if ((ShmCHAdeMOData = shmat(MeterSMId, NULL, 0)) == (void *) -1) {
			#ifdef SystemLogMessage
			DEBUG_ERROR("shmat ShmCHAdeMOData NG \n");
			#endif
			return FAIL;
		}
	}

	if(CCS_QUANTITY > 0)
	{
		if ((MeterSMId = shmget(ShmCcsCommKey, sizeof(struct CcsData),	IPC_CREAT | 0777)) < 0)
		{
			#ifdef SystemLogMessage
			DEBUG_ERROR("shmget ShmCcsData NG \n");
			#endif
			return FAIL;
		}
		else if ((ShmCcsData = shmat(MeterSMId, NULL, 0)) == (void *) -1)
		{
			#ifdef SystemLogMessage
		   	DEBUG_ERROR("shmat ShmCcsData NG \n");
			#endif
			return FAIL;
		}
	}


	return result;
}

int InitComPort()
{
	int fd;
	struct termios tios;

	fd = open(relayRs485PortName, O_RDWR);
	if(fd <= 0)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("Module_InternalComm. InitComPort NG\n");
		#endif
		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.CsuInitFailed=1;
		}
		sleep(5);
		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]=(byte)0;		// timeout 0.5 second
	tios.c_lflag=0;
	tcflush(fd, TCIFLUSH);
	ioctl (fd, TCSETS, &tios);

	return fd;
}

//================================================
// Main process
//================================================
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;
}

void Initialization()
{
	bool isPass = false;

	for (byte index = 0; index < ARRAY_SIZE(outputRelay.relay_event.relay_status); index++)
	{
		outputRelay.relay_event.relay_status[index] = 0x00;
	}

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

bool IsNoneMatchRelayStatus()
{
	bool result = false;

//	printf("Real Relay, AC = %x, g1_p = %x, g1_n = %x, g2_p = %x, g2_n = %x, pre = %x, bri_p = %x, bri_n = %x \n",
//			regRelay.relay_event.bits.AC_Contactor,
//			regRelay.relay_event.bits.Gun1_P,
//			regRelay.relay_event.bits.Gun1_N,
//			regRelay.relay_event.bits.Gun2_P,
//			regRelay.relay_event.bits.Gun2_N,
//			regRelay.relay_event.bits.CCS_Precharge,
//			regRelay.relay_event.bits.Gun1_Parallel_P,
//			regRelay.relay_event.bits.Gun1_Parallel_N);

	if ((regRelay.relay_event.bits.AC_Contactor != outputRelay.relay_event.bits.AC_Contactor) ||
		(regRelay.relay_event.bits.CCS_Precharge != outputRelay.relay_event.bits.CCS_Precharge) ||
		(regRelay.relay_event.bits.Gun1_P != outputRelay.relay_event.bits.Gun1_P) ||
		(regRelay.relay_event.bits.Gun1_N != outputRelay.relay_event.bits.Gun1_N) ||
		(regRelay.relay_event.bits.Gun2_P != outputRelay.relay_event.bits.Gun2_P) ||
		(regRelay.relay_event.bits.Gun2_N != outputRelay.relay_event.bits.Gun2_N) ||
		(regRelay.relay_event.bits.Gun1_Parallel_P != outputRelay.relay_event.bits.Gun1_Parallel_P) ||
		(regRelay.relay_event.bits.Gun1_Parallel_N != outputRelay.relay_event.bits.Gun1_Parallel_N))
	{
		result = true;
	}

	return result;
}

void MatchRelayStatus()
{
	// 因為 AC Contactor 沒有 Feedback，所以暫時先這樣處理
	//regRelay.relay_event.bits.AC_Contactor = outputRelay.relay_event.bits.AC_Contactor;
	ShmSysConfigAndInfo->SysInfo.AcContactorStatus  = regRelay.relay_event.bits.AC_Contactor = outputRelay.relay_event.bits.AC_Contactor;
	regRelay.relay_event.bits.CCS_Precharge = outputRelay.relay_event.bits.CCS_Precharge;
	regRelay.relay_event.bits.Gun1_P = outputRelay.relay_event.bits.Gun1_P;
	regRelay.relay_event.bits.Gun1_N = outputRelay.relay_event.bits.Gun1_N;
	regRelay.relay_event.bits.Gun2_P = outputRelay.relay_event.bits.Gun2_P;
	regRelay.relay_event.bits.Gun2_N = outputRelay.relay_event.bits.Gun2_N;
	regRelay.relay_event.bits.Gun1_Parallel_P = outputRelay.relay_event.bits.Gun1_Parallel_P;
	regRelay.relay_event.bits.Gun1_Parallel_N = outputRelay.relay_event.bits.Gun1_Parallel_N;
}

void CheckRelayStatusByADC()
{
	if (ShmRelayModuleData->Gun1FuseOutputVolt > 0 && ShmRelayModuleData->Gun1RelayOutputVolt > 0 &&
			(ShmRelayModuleData->Gun1FuseOutputVolt == ShmRelayModuleData->Gun1RelayOutputVolt))
	{
		// Relay 前後電壓一致
		_chargingData[0]->RelayK1K2Status = 0x01;
	}
	else
		_chargingData[0]->RelayK1K2Status = 0x00;

	if (ShmRelayModuleData->Gun2FuseOutputVolt > 0 && ShmRelayModuleData->Gun2RelayOutputVolt > 0 &&
				(ShmRelayModuleData->Gun2FuseOutputVolt == ShmRelayModuleData->Gun2RelayOutputVolt))
	{
		// Relay 前後電壓一致
		_chargingData[1]->RelayK1K2Status = 0x01;
	}
	else
		_chargingData[1]->RelayK1K2Status = 0x00;
}

void SetGfdConfig(byte index, short resister)
{
	gfd_config.index = index;
	gfd_config.reqVol = _chargingData[index]->EvBatterytargetVoltage;
	gfd_config.resister = resister;

	//printf("************************GFD Vol = %d, GFD Res = %d \n", gfd_config.reqVol, gfd_config.resister);
	if (Config_Gfd_Value(Uart5Fd, Addr.Relay, &gfd_config) == PASS)
	{
//		printf("Set reqVol = %f, resister = %d \n",
//				gfd_config.reqVol,
//				gfd_config.resister);
	}
}

void CableCheckDetected(byte index)
{
	// Cable Check
	// 當火線上的電壓 = 車端要求的電壓電流
	// _chargingData[targetGun]->EvBatterytargetVoltage
	// 才可以開始偵測 1s
	// Warning : Rgfd <= 150 歐/V 假設電壓為 500V 則~ Rgfd <= 75000 歐
	// Pre-Warning : 150 歐/V < Rgfd <= 500 歐/V 假設電壓為 500V 則 75000 歐 < Rgfd <= 250000
	// SO Normal : Rgfd > 500 歐/V 假設電壓為 500 V 則 Rgfd > 250000 歐
	if ((_chargingData[index]->SystemStatus >= S_PREPARING_FOR_EVSE && _chargingData[index]->SystemStatus <= S_CHARGING) ||
			(_chargingData[index]->SystemStatus >= S_CCS_PRECHARGE_ST0 && _chargingData[index]->SystemStatus <= S_CCS_PRECHARGE_ST1))
	{
		// Cable check
		SetGfdConfig(index, GFD_VALUE);
	}
	else if(_chargingData[index]->SystemStatus == S_COMPLETE || _chargingData[index]->SystemStatus == S_PREPARNING
			|| _chargingData[index]->SystemStatus == S_IDLE)
	{
		SetGfdConfig(index, 0);
	}
}

int main(void)
{
	if(InitShareMemory() == FAIL)
	{
		#ifdef SystemLogMessage
		DEBUG_ERROR("InitShareMemory NG\n");
		#endif
		if(ShmStatusCodeData!=NULL)
		{
			ShmStatusCodeData->AlarmCode.AlarmEvents.bits.FailToCreateShareMemory=1;
		}
		sleep(5);
		return 0;
	}
	// Open Uart5 for RB
	Uart5Fd = InitComPort();
	Initialization();
	sleep(1);

	if(Uart5Fd < 0)
	{
		printf ("open port error. \n");
		return 0;
	}

	outputRelay.relay_event.bits.AC_Contactor = 0x00;
	//outputRelay.relay_event.bits.CCS_Precharge = 0x01;
	//outputRelay.relay_event.bits.Gun1_Parallel_P = 0x01;
	//outputRelay.relay_event.bits.Gun1_Parallel_N = 0x01;
	//outputRelay.relay_event.bits.Gun1_P = 0x00;
	//outputRelay.relay_event.bits.Gun1_N = 0x00;
	//outputRelay.relay_event.bits.Gun2_P = 0x01;
	//outputRelay.relay_event.bits.Gun2_N = 0x01;
	Config_Relay_Output(Uart5Fd, Addr.Relay, &outputRelay);

	gettimeofday(&_priority_time, NULL);
	for(;;)
	{
		// 程序開始之前~ 必須先確定 FW 版本與硬體版本，確認後!!~ 該模組才算是真正的 Initial Comp.
		if (ShmRelayModuleData->SelfTest_Comp == NO)
		{
			printf("(RB) Get Fw and Hw Ver. \n");
			GetFwAndHwVersion_Relay();
			GetFwAndHwVersion_Fan();
			sleep(1);
		}
		else if (ShmRelayModuleData->SelfTest_Comp == YES)
		{
			// ==============優先權最高 10 ms ==============
			// 輸出電壓
			GetPersentOutputVol();

			// 三相輸入電壓
			GetPresentInputVol();

			// 讀取當前 relay 狀態
			GetRelayOutputStatus();

			bool isCharging = false;
			for (int i = 0; i < gunCount; i++)
			{
				// Cable check (Set)
				//CableCheckDetected(i);

				// check k1 k2 relay 狀態
				CheckK1K2RelayOutput(i);

				// 依據當前各槍的狀態選擇 搭上/放開 Relay
				SetK1K2RelayStatus(i);

				if (_chargingData[i]->SystemStatus == S_IDLE)
					gfdChkFailCount[i] = 0;

				if (_chargingData[i]->SystemStatus == S_BOOTING	||
					(_chargingData[i]->SystemStatus >= S_PREPARNING && _chargingData[i]->SystemStatus <= S_COMPLETE) ||
					(_chargingData[i]->SystemStatus >= S_CCS_PRECHARGE_ST0 && _chargingData[i]->SystemStatus <= S_CCS_PRECHARGE_ST1))
				{
					_chargingData[i]->IsReadyToCharging = YES;
					isCharging = true;
				}
				else
					_chargingData[i]->IsReadyToCharging = NO;
			}
			// Cable check (Get)
			//GetGfdAdc();

			SetParalleRelayStatus();

			// 搭上 AC Contactor
			if (isCharging)
				outputRelay.relay_event.bits.AC_Contactor = YES;
			else
				outputRelay.relay_event.bits.AC_Contactor = NO;

			// 搭上/鬆開 Relay
			// 放開 Relay 之前要先確認輸出的電壓電流是否已經降到某個值
			if(IsNoneMatchRelayStatus())
			{
				if (Config_Relay_Output(Uart5Fd, Addr.Relay, &outputRelay))
				{
					printf("Match Relay, AC = %x, g1_p = %x, g1_n = %x, g2_p = %x, g2_n = %x, pre = %x, bri_p = %x, bri_n = %x \n",
							regRelay.relay_event.bits.AC_Contactor,
							regRelay.relay_event.bits.Gun1_P,
							regRelay.relay_event.bits.Gun1_N,
							regRelay.relay_event.bits.Gun2_P,
							regRelay.relay_event.bits.Gun2_N,
							regRelay.relay_event.bits.CCS_Precharge,
							regRelay.relay_event.bits.Gun1_Parallel_P,
							regRelay.relay_event.bits.Gun1_Parallel_N);
				}
			}
			// 風扇控制
			//GetFanSpeed();

			if (GetTimeoutValue(_priority_time) / 1000 >= 1000)
			{
				gettimeofday(&_priority_time, NULL);
				if (isCharging)
				{
					if (ShmFanModuleData->PresentFan1Speed < MAX_FAN_SPEED)
					{
						ShmFanModuleData->SetFan1Speed = MAX_FAN_SPEED;
						ShmFanModuleData->SetFan2Speed = MAX_FAN_SPEED;
						ShmFanModuleData->SetFan3Speed = MAX_FAN_SPEED;
						ShmFanModuleData->SetFan4Speed = MAX_FAN_SPEED;
					}
				}
				else
				{
					if (ShmFanModuleData->PresentFan1Speed > MIN_FAN_SPEED)
					{
						ShmFanModuleData->SetFan1Speed = MIN_FAN_SPEED;
						ShmFanModuleData->SetFan2Speed = MIN_FAN_SPEED;
						ShmFanModuleData->SetFan3Speed = MIN_FAN_SPEED;
						ShmFanModuleData->SetFan4Speed = MIN_FAN_SPEED;
					}
				}
				//SetFanModuleSpeed();
			}
		}
		usleep(10000);
	}

	return FAIL;
}