csu3_am335x/EVSE/Modularization/Infypwr_PsuCommObj.c

/*
 * Infypwr_PsuCommObj.c
 *
 *  Created on: 2019年11月26日
 *      Author: 7564
 */

#include "Infypwr_PsuCommObj.h"

#define DEBUG_LIB						1

void PRINTF_LIB_FUNC(char *string, ...);
float IEEE_754_to_float(const byte raw[4]);
void IEEE_754_to_bytes(float target, byte *bytes2);

//================================================
// Private function
//================================================
void PRINTF_LIB_FUNC(char *string, ...)
{
	if (DEBUG_LIB)
	{
		va_list args;
		char buffer[4096];

		va_start(args, string);
		vsnprintf(buffer, sizeof(buffer), string, args);
		va_end(args);
		printf("%s \n", buffer);
	}
}

float IEEE_754_to_float(const byte raw[4])
{
    int sign = (raw[0] >> 7) ? -1 : 1;

    byte exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;

    unsigned int fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];

    float fraction = 0.5f;
    for (byte ii = 0; ii < 24; ++ii)
        fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));

    float significand = sign * fraction;

    return ldexpf(significand, exponent);
}

void IEEE_754_to_bytes(float target, byte *bytes2)
{
	int value2 = 0;
	number.f = target;
	int index = 31;

	value2 |= number.raw.sign << index;
	int k;
	for (k = 8 - 1; k >= 0; k--)
	{
		index--;
		if ((number.raw.exponent >> k) & 1)
			value2 |= 1 << index;
	}

	for (k = 23 - 1; k >= 0; k--)
	{
		index--;
		if ((number.raw.mantissa >> k) & 1)
			value2 |= 1 << index;
	}

	*(bytes2) = (value2 >> 24) & 0xFF;
	*(bytes2 + 1) = (value2 >> 16) & 0xFF;
	*(bytes2 + 2) = (value2 >> 8) & 0xFF;
	*(bytes2 + 3) = value2 & 0xFF;
}

//================================================
// Callback function
//================================================
void RefreshStatus(void *func)
{
	return_status = func;
}

void RefreshModuleCount(void *func)
{
	return_module_count = func;
}

void RefreshAvailableCap(void *func)
{
	return_available_cap = func;
}

void RefreshFwVersion(void *func)
{
	return_fw_version = func;
}

void RefreshInputVol(void *func)
{
	return_input_vol = func;
}

void RefreshGetOutput(void *func)
{
	return_get_output = func;
}

void RefreshGetOutputF(void *func)
{
	return_get_output_float = func;
}

void RefreshFanInfo(void *func)
{
	return_fanspeed_info = func;
}

void RefreshIavailable(void *func)
{
	return_iavail_info = func;
}

void AutoMode_RefreshOutputAndTemp(void *func)
{
	return_output_temp = func;
}

void AutoMode_RefreshModuleStatus(void *func)
{
	return_module_status = func;
}

void AutoMode_RefreshModuleInput(void *func)
{
	return_module_input = func;
}

//================================================
// CANBUS initialization
//================================================
int InitCanBus()
{
	int 					s0,nbytes;
	struct timeval			tv;
	struct ifreq 			ifr0;
	struct sockaddr_can		addr0;

	system("/sbin/ip link set can1 down");
	system("/sbin/ip link set can1 type can bitrate 500000 restart-ms 100");
	system("/sbin/ip link set can1 up");

	s0 = socket(PF_CAN, SOCK_RAW, CAN_RAW);

	tv.tv_sec = 0;
	tv.tv_usec = 10000;
   	if (setsockopt(s0, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, sizeof(struct	timeval)) < 0)
	{
		#ifdef SystemLogMessage
   		PRINTF_LIB_FUNC("Set SO_RCVTIMEO NG");
		#endif
	}
	nbytes=40960;
	if (setsockopt(s0, SOL_SOCKET,  SO_RCVBUF, &nbytes, sizeof(int)) < 0)
	{
		#ifdef SystemLogMessage
		PRINTF_LIB_FUNC("Set SO_RCVBUF NG");
		#endif
	}
	nbytes=40960;
	if (setsockopt(s0, SOL_SOCKET, SO_SNDBUF, &nbytes, sizeof(int)) < 0)
	{
		#ifdef SystemLogMessage
		PRINTF_LIB_FUNC("Set SO_SNDBUF NG");
		#endif
	}

   	strcpy(ifr0.ifr_name, "can1" );
	ioctl(s0, SIOCGIFINDEX, &ifr0); /* ifr.ifr_ifindex gets filled with that device's index */
	addr0.can_family = AF_CAN;
	addr0.can_ifindex = ifr0.ifr_ifindex;
	bind(s0, (struct sockaddr *)&addr0, sizeof(addr0));
	return s0;
}

//================================================
// Receive Cmd from canbus
//================================================
void ReceiveDataFromCanBus()
{
	int nbytes;
	struct can_frame frame;
	int intCmd = 0;
	byte group, address;

	while(1)
	{
		memset(&frame, 0, sizeof(struct can_frame));
		nbytes = read(CanFd, &frame, sizeof(struct can_frame));

		if (nbytes > 0)
		{
			frame.can_id = frame.can_id & CAN_EFF_MASK;
			intCmd = frame.can_id & 0x00FF0000;
			intCmd |= INFYPWR_GROUP_SHIFT | intCmd;

			switch (intCmd)
			{
				case WALK_IN_MODE:
				case INFYPWR_GROUP_SHIFT | WALK_IN_MODE:
				{
					//address = frame.can_id & 0x000000FF;
//					printf("walk in response address = %d, [0] = %d, [1] = %d, [2] = %d, [3] = %d, [4] = %d, [5] = %d, [6] = %d, [7] = %d \n"
//							, address,
//							frame.data[0], frame.data[1],
//							frame.data[2], frame.data[3],
//							frame.data[4], frame.data[5],
//							frame.data[6], frame.data[7]);
				}
					break;

				case INFYPWR_GROUP_SHIFT | STATUS:
				{
					group = frame.data[2];
					address = frame.can_id & 0x000000FF;

					short temp = frame.data[4];
					int status = (frame.data[5] << 16) + (frame.data[6] << 8) + frame.data[7];

					return_status(group, address, temp, status);
					//PRINTF_LIB_FUNC("group = %d, address = %d, temp = %d \n", group, address, temp);
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_COUNT:
				{
					// 回傳模組數量
					group = frame.can_id & 0x000000FF;

					byte count = frame.data[2];

					return_module_count(group, count);
					//PRINTF_LIB_FUNC("group = %d, count = %d \n", group, count);
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_CAP:
				{
					// 回傳輸出能力 : 最大電壓、最小電壓、最大電流、額定功率
					address = frame.can_id & 0x000000FF;

					short maxVol = ((frame.data[0] << 8) + frame.data[1]) * 10;
					short minVol = ((frame.data[2] << 8) + frame.data[3]) * 10;
					short maxCur = (frame.data[4] << 8) + frame.data[5];
					short totalPow = ((frame.data[6] << 8) + frame.data[7]) / 10;

					return_available_cap(address, maxVol, minVol, maxCur, totalPow);
//					PRINTF_LIB_FUNC("address = %d, maxVol = %d, minVol = %d, maxCur = %d, totalPow = %d \n",
//							address, maxVol, minVol, maxCur, totalPow);
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_OUTPUT_VOL_CUR:
				{
					// 回傳當前輸出電壓電流
					address = frame.can_id & 0x000000FF;

					int outputVol = ((frame.data[0] << 24) + (frame.data[1] << 16) + (frame.data[2] << 8) + frame.data[3]) / 100;
					int outputCur = ((frame.data[4] << 24) + (frame.data[5] << 16) + (frame.data[6] << 8) + frame.data[7]) / 100;

					return_get_output(address, outputVol, outputCur);
					//PRINTF_LIB_FUNC("address = %d, outputVol = %d, outputCur = %d \n", address, outputVol, outputCur);
				}
					break;
				case INFYPWR_GROUP_SHIFT | MODULE_OUTPUT_VOL_CUR_FLOAT:
				{
					group = frame.can_id & 0x000000FF;

					byte vol[4], cur[4];
					memcpy(vol, frame.data, 4);
					memcpy(cur, frame.data + 4, 4);

					float _Vol = IEEE_754_to_float(vol);
					float _Cur = IEEE_754_to_float(cur);

					return_get_output_float(group, _Vol, _Cur);
				}
					break;
				case INFYPWR_GROUP_SHIFT | MODULE_IAVAILABLE:
				case MODULE_IAVAILABLE:
				{
					// 回傳降載後的電流
					address = frame.can_id & 0x000000FF;

					unsigned short vextVol = ((frame.data[0] << 8) + frame.data[1]);
					unsigned short iAvailCur = ((frame.data[2] << 8) + frame.data[3]);

					return_iavail_info(address, iAvailCur, vextVol);
					//PRINTF_LIB_FUNC("address = %d, iAvailCur = %d \n", address, iAvailCur);
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_MIS_INFO:
				{
					address = frame.can_id & 0x000000FF;
					float FanSpeed;
					byte value[4];

					memcpy(value, frame.data + 4, sizeof(value));
					if (frame.data[0] == ((FAN_SPEED_CMD >> 8) & 0xFF) && frame.data[1] == (FAN_SPEED_CMD & 0xFF))
					{
						FanSpeed = IEEE_754_to_float(value);
						return_fanspeed_info(address, FanSpeed);
						//PRINTF_LIB_FUNC("address = %d, FanSpeed = %f \n", address, FanSpeed);
					}
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_VER:
				{
					// 回傳版號 : 無系統回覆功能
					address = frame.can_id & 0x000000FF;

					short dcSwVer = ((frame.data[0] << 8) + frame.data[1]);
					short pfcSwVer = ((frame.data[2] << 8) + frame.data[3]);
					short hwVer = ((frame.data[4] << 8) + frame.data[5]);

					return_fw_version(address, dcSwVer, pfcSwVer, hwVer);
					//PRINTF_LIB_FUNC("address = %d, DC %d, PFC %d, HW %d \n", address, dcSwVer, pfcSwVer, hwVer);
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_BARCODE:
				{
					// 回傳BarCode
				}
					break;

				case INFYPWR_GROUP_SHIFT | MODULE_INPUT:
				{
					// 回傳三向輸入電壓
					address = frame.can_id & 0x000000FF;

					short abVol = ((frame.data[0] << 8) + frame.data[1]) / 10;
					short bcVol = ((frame.data[2] << 8) + frame.data[3]) / 10;
					short caVol = ((frame.data[4] << 8) + frame.data[5]) / 10;

					return_input_vol(address, abVol, bcVol, caVol);
					//PRINTF_LIB_FUNC("address = %d, abVol = %d, bcVol = %d, caVol = %d \n", address, abVol, bcVol, caVol);
				}
					break;
				case INFYPWR_GROUP_SHIFT | AUTO_OUTPUT_TEMP:
				{
					/*Test mode used*/
					// 回傳輸出值與入風口溫度
					address = frame.can_id & 0x000000FF;

					short outputVol = ((frame.data[0] << 8) + frame.data[1]);
					short outputCur = ((frame.data[2] << 8) + frame.data[3]);
					short outputPow = ((frame.data[4] << 8) + frame.data[5]);
					byte temp = frame.data[6];

					return_output_temp(address, outputVol, outputCur, outputPow, temp);
					//PRINTF_LIB_FUNC("address = %d, abVol = %d, bcVol = %d, caVol = %d \n", address, abVol, bcVol, caVol);
				}
					break;
				case INFYPWR_GROUP_SHIFT | AUTO_MODULE_STATUS:
				{
					/*Test mode used*/
					// 回傳輸出值與入風口溫度
					address = frame.can_id & 0x000000FF;

					byte isErr =  (frame.data[0] >> 0) & 0x01;
					byte status = (frame.data[0] >> 1) & 0x01;
					byte err1 = frame.data[2];
					byte err2 = frame.data[3];
					byte err3 = frame.data[4];
					byte err4 = frame.data[5];

					return_module_status(address, isErr, status, err1, err2, err3, err4);
					//PRINTF_LIB_FUNC("address = %d, abVol = %d, bcVol = %d, caVol = %d \n", address, abVol, bcVol, caVol);
				}
					break;
				case INFYPWR_GROUP_SHIFT | AUTO_MODULE_INPUT:
				{
					/*Test mode used*/
					// 回傳輸出值與入風口溫度
					address = frame.can_id & 0x000000FF;

					short vR = ((frame.data[0] << 8) + frame.data[1]);
					short vS = ((frame.data[2] << 8) + frame.data[3]);
					short vT = ((frame.data[4] << 8) + frame.data[5]);

					return_module_input(address, vR, vS, vT);
					//PRINTF_LIB_FUNC("address = %d, abVol = %d, bcVol = %d, caVol = %d \n", address, abVol, bcVol, caVol);
				}
					break;
			}
		}
		usleep(10000);
	}
}

//================================================
// Private Function
//================================================
void SendCmdToPsu(int cmd, byte *data, byte dataLen)
{
    struct can_frame frame;

    //設定 CANBSU 2.0B 長封包
    cmd = cmd | 0x80000000;

    frame.can_id = cmd;
    frame.can_dlc = dataLen;
    memcpy(frame.data, data, dataLen);

    write(CanFd, &frame, sizeof(struct can_frame));
	usleep(CMD_DELAY_TIME);
}

bool InitialCommunication()
{
	CanFd = InitCanBus();

	if(CanFd < 0)
	{
		PRINTF_LIB_FUNC("Init can bus fail.\n");
		return false;
	}

    recFork = fork();
    if(recFork > 0)
    {
    	ReceiveDataFromCanBus();
	}
    else if(recFork > 0)
    {
    	PRINTF_LIB_FUNC("fork fail\n");
    }

    return true;
}

//================================================
// API Function
//================================================
void SwitchPower(byte group, byte value)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | SWITCH_POWER;

	memset(data, 0x00, ARRAY_SIZE(data));
	// 1 : 關機
	// 0 : 開機
	data[0] = value;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void SleepMode(byte group, byte value)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | SLEEP_MODE;

	memset(data, 0x00, ARRAY_SIZE(data));
	// 1 : 休眠
	// 0 : 起床
	data[0] = value;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void FlashLed(byte group, byte value)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | FLASH_LED;

	memset(data, 0x00, ARRAY_SIZE(data));
	// 1 : 閃爍
	// 0 : 正常
	data[0] = value;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void PresentOutputVol(byte group, int voltage, int current)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | PRESENT_OUT_VOL;
	int Vol = voltage * 100;
	int Cur = current * 100;

	memset(data, 0x00, ARRAY_SIZE(data));

	// 輸出電壓
	data[0] = (Vol >> 24) & 0xFF;
	data[1] = (Vol >> 16) & 0xFF;
	data[2] = (Vol >> 8) & 0xFF;
	data[3] = Vol & 0xFF;
	// 輸出電流
	data[4] = (Cur >> 24) & 0xFF;
	data[5] = (Cur >> 16) & 0xFF;
	data[6] = (Cur >> 8) & 0xFF;
	data[7] = Cur & 0xFF;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void FanNoiseInfo(byte group, byte value)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MIS_INFO;

	memset(data, 0x00, ARRAY_SIZE(data));

	// 風扇低噪音
	data[0] = 0x11;
	data[1] = 0x13;

	// 0xA0 power poriority mode
	// 0xA1 denoise mode
	// 0xA2 quiet mode
	data[7] = value;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}


void SetWalkInConfig(byte group, byte enable, byte sec)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | WALK_IN_MODE;

	memset(data, 0x00, ARRAY_SIZE(data));
	unsigned short _Sec = sec * 100;

	// Walk-in mode enable
	data[0] = enable;
	// Walk-in time (default == 5s)
	data[6] = (_Sec >> 8) & 0xFF;
	data[7] = _Sec & 0xFF;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= (group << 8) | INFYPWR_DEFAULT;

	//printf("walk in cmd = %x \n", cmd);
	SendCmdToPsu(cmd, data, sizeof(data));
}

void SetDirModulePresentOutput(byte group, int voltage, int current, byte _switch, byte _interRelay)
{
	byte data[8];
	uint cmd = TEST_PRESENT_OUT;  //0x180100E5

	memset(data, 0x00, ARRAY_SIZE(data));

	// 輸出電壓
	data[0] = (voltage >> 8) & 0xFF;
	data[1] = voltage & 0xFF;
	// 輸出電流
	data[2] = (current >> 8) & 0xFF;
	data[3] = current & 0xFF;
	// 開 / 關
	data[4] = _switch;
	// Internal Relay
	data[5] = _interRelay;

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST;
	else
		cmd |= (group << 8);
	SendCmdToPsu(cmd, data, sizeof(data));
}

/**********************************************************************************/
/***                                                                            ***/
/***                                   Get                                      ***/
/***                                                                            ***/
/**********************************************************************************/
void GetStatus(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | STATUS;

	memset(data, 0x00, ARRAY_SIZE(data));

	cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetFanSpeed(byte group)
{
	uint cmd;
	byte data[8];

	cmd = INFYPWR_CMD | MODULE_MIS_INFO;

	memset(data, 0x00, ARRAY_SIZE(data));

	data[0] = (FAN_SPEED_CMD >> 8) & 0xFF;
	data[1] = FAN_SPEED_CMD & 0xFF;

	if (group == (INFYPWR_BROADCAST >> 8))
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleCount(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_COUNT;

	memset(data, 0x00, ARRAY_SIZE(data));

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleVer(byte group)
{
	// 無系統廣播功能
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_VER;

	memset(data, 0x00, ARRAY_SIZE(data));

	cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	//PRINTF_LIB_FUNC("GetModuleVer cmd = %x\n", cmd);
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleCap(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_CAP;

	memset(data, 0x00, ARRAY_SIZE(data));

	cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	//PRINTF_LIB_FUNC("GetModuleCap cmd = %x\n", cmd);
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleBarCode(byte group)
{
	// 無系統廣播功能
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_BARCODE;

	memset(data, 0x00, ARRAY_SIZE(data));

	cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleInput(byte group)
{
	// 無系統廣播功能
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_INPUT;

	memset(data, 0x00, ARRAY_SIZE(data));

	cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleIavailable(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_IAVAILABLE;

	memset(data, 0x00, ARRAY_SIZE(data));

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleOutput(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_OUTPUT_VOL_CUR;

	memset(data, 0x00, ARRAY_SIZE(data));

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

void GetModuleOutputF(byte group)
{
	byte data[8];
	uint cmd = INFYPWR_CMD | MODULE_OUTPUT_VOL_CUR_FLOAT;

	memset(data, 0x00, ARRAY_SIZE(data));

	if (group == SYSTEM_CMD)
		cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	else
		cmd |= INFYPWR_GROUP_SHIFT | (group << 8) | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}

/**********************************************************************************/
/***                                                                            ***/
/***                                 Upgrate                                    ***/
/***                                                                            ***/
/**********************************************************************************/
void ChangePsuBaudrate(short baudrate)
{
	byte data[8];
	uint cmd = CHANGE_BAUDRATE;  //0x180100E5

	memset(data, 0x00, ARRAY_SIZE(data));

	data[0] = 0x11;
	data[1] = 0x26;

	if (baudrate == 125)
		data[7] = 0xA0;
	else if (baudrate == 250)
		data[7] = 0xA1;
	else if (baudrate == 500)
		data[7] = 0xA2;

	cmd |= INFYPWR_BROADCAST | INFYPWR_DEFAULT;
	SendCmdToPsu(cmd, data, sizeof(data));
}