#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <sys/shm.h>
#include <semaphore.h>
#include <sys/sem.h>
#include <errno.h>
#include <unistd.h>
#include <time.h>
#include <fcntl.h>
#include <stdbool.h>
#include <sys/ioctl.h>
#include <math.h>
#include <signal.h>
#include <sys/syscall.h>
#include <pthread.h>

#include "EcatDeviceAPI.h"
#include "EcatDeviceAPI_Ex.h"
#include "EcatDeviceAPI_StickControl.h"
#include "EcatErrno.h"
#include "EcatGlobals.h"
#include "ixecat.h"
#include "config.h"

#include "ezxml.h"

// bar0 reg offset
#define GPIO_ADDR_OFFSET 						0x000
#define CARD_ID_ADDR_OFFSET					0x004
#define CMP_TRIG_COMM_CONFIG_ADDR_OFFSET 0x00C
#define ENC0_MODE_ADDR_OFFSET 	0x010
#define ENC0_CNT_ADDR_OFFSET 		0x014
#define ENC1_MODE_ADDR_OFFSET 	0x018
#define ENC1_CNT_ADDR_OFFSET 		0x01C
#define ENC0_CMP_TRIG_DATA_ADDR_OFFSET 		0x020
#define ENC0_CMP_TRIG_PITCH_ADDR_OFFSET 		0x024
#define ENC0_CMP_TRIG_TIMES_ADDR_OFFSET 		0x028
#define ENC0_CMP_TRIG_CONFIG_ADDR_OFFSET 		0x02C
#define ENC1_CMP_TRIG_DATA_ADDR_OFFSET 		0x034
#define ENC1_CMP_TRIG_PITCH_ADDR_OFFSET 		0x038
#define ENC1_CMP_TRIG_TIMES_ADDR_OFFSET 		0x03C
#define ENC1_CMP_TRIG_CONFIG_ADDR_OFFSET 		0x040
#define DIRECTLY_ZONE_OFFSET 					0x048

#define REG_COUNT			0x400
#define REG_COMMAND			0x404
#define REG_DATA			0x408
#define REG_STATUS_HOST		0x40C
#define REG_COUNT_CONF		0x410
#define REG_COMMAND_CONF	0x414
#define REG_DATA_CONF		0x418
#define REG_STATUS_DEVICE	0x41C

// Host Status
#define STA_COMBUSY			0x00000000	// Command is not ready
#define STA_COMREADY		0x00000001	// Command is ready

//Response Status
#define STA_COMVALID		0x00000000	// Command Valid
#define STA_ERRCRC			0x00000100	// CRC Check Error
#define STA_OVERADDRESS		0x00000200	// Over Flash Address Definition
#define STA_ERRCOM			0x00000300	// Command Error
#define STA_ERRFLASHCTL		0x00000400	// Flash Control Error
#define STA_PCIECHK			0x00005A00	// PCIe Check Finished
#define STA_BUSY			0x0000FF00	// Command Busy

typedef struct ecat_slave_aio_config {
	uint8_t 		Range;
	double			VoltScaling;
}ecat_slave_aio_config_t;

/*****************************************************************************/
/** Slave data
 */
typedef struct ecat_slave {
	ecat_slave_aio_config_t AioConfig;
}ecat_slave_t;
/*****************************************************************************/
/** Slave data struct
 */
typedef struct ecat_2016 {
	int16_t Status;
	int16_t Value;
}ecat_2016_t;

//XILINX SP6 DUAL Port RAM CRC(High Byte)
static unsigned char CRC16Hi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};

//XILINX SP6štŠC DUAL Port RAM CRC(Low Byte)
static unsigned char CRC16Lo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2,
0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04,
0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8,
0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6,
0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10,
0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE,
0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA,
0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C,
0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0,
0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62,
0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C,
0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76,
0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54,
0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98,
0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A,
0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86,
0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
// bar2 reg offset
#define POLLING_FLAG_ADDR_OFFSET 0x00
#define CMD_RET_ADDR_OFFSET 0x01
#define DATA_ADDR_OFFSET 0x02
#define PC_INT_RDC_ADDR_OFFSET 0x7FF
#define DIRECTLY_DATA_OFFSET 0x200

#define DEV_ENCODER_MAX 2
#define MC_MPG_AXIS_NO_MAX                   	7

#define MC_DOUBLE_EQUAL_ZERO			0.00001

#define M_PI		3.14159265358979323846	/* pi */
#define M_2PI		6.283185307179586476925286766559

#define MC_CIRC_MODE_CP_ANGLE		0  //Center Point, Angle
#define MC_CIRC_MODE_CP_EP      1  //Center Point, End Point
#define MC_CIRC_MODE_BP_EP      2  //Border Point, End Point

#define DIFF_NS(A, B) 						(((A).tv_sec - (B).tv_sec) * NSEC_PER_SEC + (A).tv_nsec - (B).tv_nsec)
#define CHECK_ECAT_SLAVE_NO(a) if(a >= NETWORK_SLAVE_MAX) return ECAT_ERR_INVALID_SLAVE_NO;
#define CHECK_CSV_DATA_SIZE(a) if(a > ECAT_IPC_DATA_SIZE_MAX) return ECAT_ERR_INVALID_DATA_SIZE;   /*MartinK 2021_03_26*/
typedef struct stewartPlatform {
	double rb;
	double rp;
	double ab;
	double ap;
	double ab_degree;
	double ap_degree;
	double L0[6];
	double Z0;
	double angP[7];
	double Px[7];
	double Py[7];
	double Pz[7];
	double angB[7];
	double Bx[7];
	double By[7];
	double Bz[7];
	double Lmin[6];
	double Lmax[6];
	int setup;
}stewartPlatform_t;

typedef struct ecat_device_ipc {
	int Fd;
	bool IsOpen;
	sem_t	ThreadSem;
	uint16_t GrpAxisCnt[MC_GROUP_NO_MAX];
	uint16_t	MpgAxisNo[MC_MPG_AXIS_NO_MAX];
	uint16_t MpgAxisCnt;
	pthread_t MpgThreadID;
	uint8_t MpgThreadRunning;
	uint8_t MpgThreadShouldStop;
	stewartPlatform_t	StewartPlatform;
	uint8_t irq_enable;
	struct sigaction sig_act;
	pthread_cond_t  cond_timer;
	pthread_mutex_t cond_timer_mutex;
	pthread_cond_t  cond_event;
	pthread_mutex_t cond_event_mutex;
	ecat_slave_t Slave[NETWORK_SLAVE_MAX];
	uint32_t             Device_Type;
	uint16_t McSlaveNo[MC_AXIS_NO_MAX];
	uint16_t McSubAxisNo[MC_AXIS_NO_MAX];
	uint16_t McAxisCnt;
	uint64_t PosQuadTimeOut;
	double AxisPPU[MC_AXIS_NO_MAX];
	double ActualPos[MC_AXIS_NO_MAX];
	double CommandPos[MC_AXIS_NO_MAX];
	uint32_t AxisState[MC_AXIS_NO_MAX];
	uint32_t AxisDI[MC_AXIS_NO_MAX];
	uint8_t DirectlyEnable;
	uint8_t DIOmapEnable;
	uint16_t DIO_map_Cnt;
	uint16_t DIO_map_No[SLAVE_DIO_MAP_MAX];
	uint16_t DIO_map_Dir[SLAVE_DIO_MAP_MAX];
	uint32_t DIO_map_Value[SLAVE_DIO_MAP_MAX];
	uint32_t DIO_map_Value_[SLAVE_DIO_MAP_MAX];
	uint8_t Xilinx_Process;
	uint8_t LogMcMessage;
	char Logmsg[512];
}ecat_device_ipc_t;
/*****************************************************************************/
typedef union encoder_config {
	struct
	{
		uint8_t	 Mode0 : 1;
		uint8_t	 EncoderMode : 2;
		uint8_t	 NC3 : 1;
		uint8_t	 LPA1 : 1;
		uint8_t	 LPB1 : 1;
		uint8_t	 NC6_7 : 2;
		uint8_t	 LPF : 8;
		uint16_t NC16_31 : 16;
	};
	uint32_t Cfg;
}encoder_config_t;



const double DegtoRad = 0.01745329251;//   pi/180

static ecat_device_ipc_t EcatDevice[CARD_DEVICE_NO_MAX] = {{0}};

#define CHECK_ECAT_DEVICE_NO(a) if(a >= CARD_DEVICE_NO_MAX) return ECAT_ERR_IPC_INVALID_DEVICE_NO;
#define CHECK_ECAT_DEVICE_IS_OPEN(a) if(!a) return ECAT_ERR_IPC_DEVICE_NOT_OPEN;
#define CHECK_MC_PROFILE_NO(a)				if(a >= MC_PROFILE_NO_MAX) return ECAT_ERR_MC_INVALID_PROFILE_NO;
#define CHECK_MC_ECAM_TABLE_NO(a)				if(a >= MC_ECAM_TABLE_NO_MAX) return ECAT_ERR_MC_INVALID_ECAM_TABLE_NO;
#define SEMEPHORE_P_OPERATION(a) sem_wait(&a);
#define SEMEPHORE_V_OPERATION(a) sem_post(&a);
#define CHECK_PID_NO(a) if(a>=ECAT_ERR_INVALID_PID_NO || a<0) return ECAT_ERR_INVALID_PID_NO;

static struct timespec polling_time = { .tv_sec = 0 , .tv_nsec = ECAT_IPC_POLLING_NS};

static int32_t SetCommandData(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size);
static int32_t SetCommandData_OpenDevice(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size);
static int32_t SetCommandDataEx(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size, uint64_t TimeOut_ms);
static int32_t GetReturnData(ecat_device_ipc_t *EcatDevice, void *Data, uint32_t Size);
static int32_t GenericSendCommand(uint16_t device_no, uint64_t timeout_ms, int32_t cmd_no,
									void *args, uint32_t args_size, void *ret_data, uint32_t ret_size);
static int32_t GenericSendCommand_ex(uint16_t device_no, uint64_t timeout_ms, int32_t cmd_no,
									void* args, uint32_t args_size, void* ret_data, uint32_t ret_size);
static int32_t GetReturnData_Directly(ecat_device_ipc_t* EcatDevice, uint32_t offset, void* Data, uint32_t Size);
static void Vector_Cross(uint16_t Dim, double *OutVector, double *Vect1, double *Vect2);
static double Vector_Dot(uint16_t Dim, double *Vect1, double *Vect2);
static int32_t CheckNormalVector(double *Vec);
static int32_t FindNV(double *SP, double *CP, double *EP, double *NV);
static int32_t FindCP_2D(double *p1, double *p2, double *p3, double *CP);
static int32_t FindCP_3D(double *p1, double *p2, double *p3, double *CP, double *NV);
static int32_t InverseMatrix(double *a);

/*****************************************************************************/
static void timespec_add_nsec(struct timespec *r, const struct timespec *a, int64_t b)
{
	r->tv_sec = a->tv_sec + (b / NSEC_PER_SEC);
	r->tv_nsec = a->tv_nsec + (b % NSEC_PER_SEC);

	if (r->tv_nsec >= NSEC_PER_SEC) {
		r->tv_sec++;
		r->tv_nsec -= NSEC_PER_SEC;
	} else if (r->tv_nsec <= -NSEC_PER_SEC) {
		r->tv_sec--;
		r->tv_nsec += NSEC_PER_SEC;
	}
}
/*****************************************************************************/
int32_t read_bar0_reg(int fd, uint32_t offset, uint32_t *data, uint32_t size)
{
	uint32_t i;
	ixecat_reg_t reg;
	reg.id = IXECAT_HOST_B0;

	for (i = 0; i < size; i++) {
		reg.reg_off = offset + (i * 0x04);
		if (ioctl(fd, IXECAT_READ_REG, &reg))
			return ECAT_ERR_IPC_READ_SHM;
		else
			data[i] = reg.value;
	}
	return 0;

}
/*****************************************************************************/
int32_t write_bar0_reg(int fd, uint32_t offset, uint32_t *data, uint32_t size)
{
	uint32_t i;
	ixecat_reg_t reg;
	reg.id = IXECAT_HOST_B0;

	for (i = 0; i < size; i++) {
		reg.reg_off = offset + (i * 0x04);
		reg.value = data[i];
		if (ioctl(fd, IXECAT_WRITE_REG, &reg))
			return ECAT_ERR_IPC_WRITE_SHM;
	}
	return 0;

}
/*****************************************************************************/
int32_t read_dpram(int fd, uint32_t offset, uint32_t *data, uint32_t size)
{
	uint32_t i;
	ixecat_reg_t reg;
	reg.id = IXECAT_SRAM1;

	for (i = 0; i < size; i++) {
		reg.sram_off = offset + i;
		if (ioctl(fd, IXECAT_READ_REG, &reg))
			return ECAT_ERR_IPC_READ_SHM;
		else
			data[i] = reg.value;
	}
	return 0;

}
/*****************************************************************************/
int32_t write_dpram(int fd, uint32_t offset, uint32_t *data, uint32_t size)
{
	uint32_t i;
	ixecat_reg_t reg;
	reg.id = IXECAT_SRAM1;

	for (i = 0; i < size; i++) {
		reg.sram_off = offset + i;
		reg.value = data[i];
		if (ioctl(fd, IXECAT_WRITE_REG, &reg))
			return ECAT_ERR_IPC_WRITE_SHM;
	}
	return 0;

}
/*****************************************************************************/
int32_t __SetCommandData(int fd, int32_t cmd_no, uint32_t *data,uint32_t size, uint32_t flag_value, uint64_t timeout_ms)
{
	struct timespec start_time, end_time;
	uint32_t resize = size / ECAT_IPC_SHM_ALIGN;
	uint32_t flag;
	uint64_t timeout_ns;
	if (timeout_ms == 0)
		timeout_ns = ECAT_IPC_TIMEOUT_NS;
	else
		timeout_ns = timeout_ms * NSEC_PER_MSEC;


	//get polling flag
	read_dpram(fd, POLLING_FLAG_ADDR_OFFSET, &flag, 1);
	if (flag != 0)
		return ECAT_ERR_IPC_BUSY;

	//set command No.
	write_dpram(fd, CMD_RET_ADDR_OFFSET, &cmd_no, 1);

	//set oprations data
	if(data != NULL)
		write_dpram(fd, DATA_ADDR_OFFSET, data, resize);

	//set polling flag
	flag = 1;
	write_dpram(fd, POLLING_FLAG_ADDR_OFFSET, &flag, 1);
	write_dpram(fd, PC_INT_RDC_ADDR_OFFSET, &flag, 1);

	//wait polling flag is clear
	clock_gettime(CLOCK_MONOTONIC, &start_time);
	do {
		read_dpram(fd, POLLING_FLAG_ADDR_OFFSET, &flag, 1);
		if (flag == flag_value)
			break;

		clock_nanosleep(CLOCK_REALTIME, 0, &polling_time, NULL);
		clock_gettime(CLOCK_MONOTONIC, &end_time);
		if(DIFF_NS(end_time, start_time) >= timeout_ns) {
			flag = 0;
			write_dpram(fd, POLLING_FLAG_ADDR_OFFSET, &flag, 1);
			write_dpram(fd, PC_INT_RDC_ADDR_OFFSET, &flag, 1);
			return ECAT_ERR_IPC_TIME_OUT;
		}

	} while(flag != flag_value);

	return 0;
}
/*****************************************************************************/
int32_t SetCommandData(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size)
{
	return __SetCommandData(EcatDevice->Fd, CmdNo, (uint32_t *)Data, Size, 0, 0);
}
/*****************************************************************************/
int32_t SetCommandData_OpenDevice(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size)
{
	return __SetCommandData(EcatDevice->Fd, CmdNo, (uint32_t *)Data, Size, 2, 0);
}
/*****************************************************************************/
int32_t SetCommandDataEx(ecat_device_ipc_t *EcatDevice, int32_t CmdNo, void *Data, uint32_t Size, uint64_t TimeOut_ms)
{
	return __SetCommandData(EcatDevice->Fd, CmdNo, (uint32_t *)Data, Size, 0, TimeOut_ms);
}
/*****************************************************************************/
int32_t __GetReturnData(int fd, uint32_t *data,uint32_t size)
{
	uint32_t resize = size / ECAT_IPC_SHM_ALIGN;
	int32_t value;

	//get return value
	read_dpram(fd, CMD_RET_ADDR_OFFSET, &value, 1);

	//get return data
	if(data != NULL)
		read_dpram(fd, DATA_ADDR_OFFSET, data, resize);

	return value;
}
///*****************************************************************************/
int32_t GetReturnData(ecat_device_ipc_t *EcatDevice, void *Data, uint32_t Size)
{
	 return __GetReturnData(EcatDevice->Fd, (uint32_t *)Data, Size);
}
/*****************************************************************************/
int32_t GenericSendCommand(uint16_t device_no, uint64_t timeout_ms, int32_t cmd_no, void *args, uint32_t args_size, void *ret_data, uint32_t ret_size)
{
	CHECK_ECAT_DEVICE_NO(device_no)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[device_no].IsOpen)
	int32_t ret;
	SEMEPHORE_P_OPERATION(EcatDevice[device_no].ThreadSem)

	if ((ret = __SetCommandData(EcatDevice[device_no].Fd, cmd_no, (uint32_t *)args, args_size, 0, timeout_ms)) < 0)
		goto out_return;

	ret = __GetReturnData(EcatDevice[device_no].Fd, (uint32_t *)ret_data, ret_size);

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[device_no].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t GenericSendCommand_ex(uint16_t device_no, uint64_t timeout_ms, int32_t cmd_no, void* args, uint32_t args_size, void* ret_data, uint32_t ret_size)
{
	CHECK_ECAT_DEVICE_NO(device_no)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[device_no].IsOpen)
	int32_t ret;

	if ((ret = __SetCommandData(EcatDevice[device_no].Fd, cmd_no, (uint32_t*)args, args_size, 0, timeout_ms)) < 0)
		goto out_return;

	ret = __GetReturnData(EcatDevice[device_no].Fd, (uint32_t*)ret_data, ret_size);

out_return:
	return ret;
}
/*****************************************************************************/
int32_t GetReturnData_Directly(ecat_device_ipc_t* EcatDevice, uint32_t offset, void* Data, uint32_t Size)//Size not used
{
	uint32_t Zone;
	int32_t ret;
	if ((ret = read_bar0_reg(EcatDevice->Fd, DIRECTLY_ZONE_OFFSET, &Zone, 1) != 0))
		return ret;
	Zone &= 0x00000003;
	if (Zone == 0)
		Zone = 3 * 0x100;
	else
		Zone = (Zone - 1) * 0x100;
	read_dpram(EcatDevice->Fd, DIRECTLY_DATA_OFFSET + offset + Zone, (uint32_t*)Data, 1);

	return 0;
}
/*****************************************************************************/
int32_t irq_setup(uint16_t DeviceNo, uint8_t Enabled)
{
	ixecat_reg_t reg;
	if (Enabled) {
		if (EcatDevice[DeviceNo].irq_enable) {
			return 0;
		}
	} else {
		if (!EcatDevice[DeviceNo].irq_enable)  {
			return 0;
		}
	}

	reg.id = IXECAT_ISR;    //CPU(or Host) side's ioctl command
	reg.value = Enabled; //enable En_ISR
	if (ioctl(EcatDevice[DeviceNo].Fd, IXECAT_WRITE_REG, &reg)) {
		return ECAT_ERR_DRV_IOCTL;
	} else {
		EcatDevice[DeviceNo].irq_enable = Enabled;
		return 0;
	}
}
/*****************************************************************************/
static void sig_handler(int sig)
{
	int DeviceNo = sig - SIGRTMIN;
	uint32_t state;
	if ((DeviceNo >= CARD_DEVICE_NO_MAX) && (DeviceNo < 0))
		return;

	read_dpram(EcatDevice[DeviceNo].Fd, 0x7FE, &state, 1);
	state = (state >> 8) & 0xFF;
	if (state & 0x01)
		pthread_cond_broadcast(&EcatDevice[DeviceNo].cond_timer);

	if (state & 0x04)
		pthread_cond_broadcast(&EcatDevice[DeviceNo].cond_event);
}
/*****************************************************************************/
int32_t ECAT_GetDllVersion(char *Version, uint16_t *Size)
{
	if (Version) {
		strcpy(Version, LINUX_LIB_VERSION);
	} else {
		if (Size)
			*Size = sizeof(LINUX_LIB_VERSION);
	}
	return 0;
}
/*****************************************************************************/
int32_t ECAT_GetFirmwareVersion(uint16_t DeviceNo, char *Version, uint16_t *Size)
{
	int32_t ret;
	ecat_firmware_info_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_FIRWARE_VERSION,
							NULL, 0, &RetData, sizeof(ecat_firmware_info_t));

	if (ret == 0) {
		if (Version)
			memcpy(Version, RetData.Data, sizeof(RetData.Data) + 1);
		else
			if (Size)
				*Size = RetData.Size;
	}
	return ret;
}
/*****************************************************************************/
int32_t __GetDeviceCnt(uint16_t *DeviceCnt)
{
	uint16_t i;
	uint16_t cnt = 0;
	char dev_file[128];
	for (i = 1; i <= CARD_DEVICE_NO_MAX; i++) {
		sprintf(dev_file, "/dev/ixecat%d", i);
		if (access( dev_file, F_OK ) != -1)
			cnt++;
	}
	if (cnt > CARD_DEVICE_NO_MAX)
		cnt = CARD_DEVICE_NO_MAX;

	*DeviceCnt = cnt;
	return 0;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceCnt(uint16_t *DeviceCnt, uint8_t CardID[])
{
	int32_t ret;
	uint16_t cnt = 0;
	uint16_t i;
	uint32_t card_id;
	int fd_dev;
	char dev_file[128];

	__GetDeviceCnt(&cnt);
	*DeviceCnt = cnt;

	for (i = 1; i <= cnt; i++) {
		sprintf(dev_file, "/dev/ixecat%d", i);
		fd_dev = open(dev_file, O_RDWR);
		if (fd_dev == -1)
			return ECAT_ERR_IPC_DEVICE_NOT_OPEN;
		else {
			if ((ret = read_bar0_reg(fd_dev, CARD_ID_ADDR_OFFSET, &card_id, 1) != 0))
				return ret;
			else {
				card_id = ~card_id & 0xF;
				CardID[i - 1] = card_id;
				close(fd_dev);
			}
		}
	}
	return 0;
}
/*****************************************************************************/
int32_t ECAT_OpenDevice(uint16_t DeviceNo)
{
	int32_t ret;
	uint16_t i;
	uint16_t cnt;
	uint32_t card_id;
	bool finde_dev = false;
	int fd_dev;
	char dev_file[128];
	uint32_t dpram_data;
	struct sigaction *sig_act = &EcatDevice[DeviceNo].sig_act;
	ixecat_signal_t sig;

	CHECK_ECAT_DEVICE_NO(DeviceNo)
	if (EcatDevice[DeviceNo].IsOpen)
		return 0;

	__GetDeviceCnt(&cnt);
	for (i = 1; i <= cnt; i++) {
		sprintf(dev_file, "/dev/ixecat%d", i);
		fd_dev = open(dev_file, O_RDWR);
		if (fd_dev == -1)
			continue;
		else {
			if ((ret = read_bar0_reg(fd_dev, CARD_ID_ADDR_OFFSET, &card_id, 1) != 0)) {
				close(fd_dev);
				continue;
			}
			else {

				card_id = ~card_id & 0xF;
				if (card_id == DeviceNo) {
					finde_dev = true;
					break;
				} else
					close(fd_dev);
			}
		}
	}

	if (!finde_dev)
		return ECAT_ERR_DRV_DEVICE_NOT_FOUND;

	sem_init(&EcatDevice[DeviceNo].ThreadSem, 0, 1);
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	EcatDevice[DeviceNo].Fd = fd_dev;
	dpram_data = 0;
	write_dpram(EcatDevice[DeviceNo].Fd, POLLING_FLAG_ADDR_OFFSET, &dpram_data, 1);

	if ((ret = SetCommandData_OpenDevice(&EcatDevice[DeviceNo], ECAT_IPC_CMD_OPEN_DEVICE, NULL, 0)) < 0) {
		goto out_return;
	}

	write_dpram(EcatDevice[DeviceNo].Fd, POLLING_FLAG_ADDR_OFFSET, &dpram_data, 1);

	ecat_firmware_info_t RetData;
/*
	if ((ret = GenericSendCommand_ex(DeviceNo, 0, ECAT_IPC_CMD_GET_FIRWARE_VERSION,
		NULL, 0, &RetData, sizeof(ecat_firmware_info_t))) < 0)
	{
		goto out_return;
	}
	*/
	if ((ret = __SetCommandData(EcatDevice[DeviceNo].Fd, ECAT_IPC_CMD_GET_FIRWARE_VERSION, NULL, 0, 0, 0)) < 0)
		goto out_return;

	if((ret = __GetReturnData(EcatDevice[DeviceNo].Fd, (uint32_t*)&RetData, sizeof(ecat_firmware_info_t))) < 0)
		goto out_return;
		
	char Version[255];
	uint16_t Size;
	memset(Version, 0, sizeof(RetData.Data));
	memcpy(Version, RetData.Data, strlen(RetData.Data));
	char* pch;
	char* next_token = NULL;
	cnt = 0;
	pch = strtok_r(Version, ".", &next_token);
	while (pch != NULL) {
		cnt++;
		if (cnt == 7)
			break;
		pch = strtok_r(NULL, ".", &next_token);
	}
	if (cnt == 7)
	{
		if (pch != NULL)
		{
			if (strcmp(pch, "901") == 0)
				EcatDevice[DeviceNo].Device_Type = 901;
			else
				EcatDevice[DeviceNo].Device_Type = 801;
		}
	}
	else
		EcatDevice[DeviceNo].Device_Type = 801;

	/* signal action */
	sig_act->sa_handler = sig_handler;
	sigemptyset(&sig_act->sa_mask);
	sigaddset(&sig_act->sa_mask, (SIGRTMIN + DeviceNo));
	if (sigaction((SIGRTMIN + DeviceNo), sig_act, NULL)) {
		ret = ECAT_ERR_DRV_CREATE_HANDLE;
		goto out_return;
	}

	if (pthread_mutex_init(&EcatDevice[DeviceNo].cond_timer_mutex, NULL)) {
		ret = ECAT_ERR_DRV_CREATE_HANDLE;
		goto out_return;
	}

	if (pthread_cond_init(&EcatDevice[DeviceNo].cond_event, NULL)) {
		ret = ECAT_ERR_DRV_CREATE_HANDLE;
		goto out_return;
	}

	if (pthread_mutex_init(&EcatDevice[DeviceNo].cond_event_mutex, NULL)) {
		ret = ECAT_ERR_DRV_CREATE_HANDLE;
		goto out_return;
	}

	if (pthread_cond_init(&EcatDevice[DeviceNo].cond_timer, NULL)) {
		ret = ECAT_ERR_DRV_CREATE_HANDLE;
		goto out_return;
	}

	/* signal condition */
	sig.sid = (SIGRTMIN + DeviceNo);
	sig.pid = getpid();
	if (ioctl(EcatDevice[DeviceNo].Fd, IXECAT_SET_SIG, &sig)) {
		ret = ECAT_ERR_DRV_IOCTL;
		goto out_return;
	}

	for (i = 0; i < MC_GROUP_NO_MAX; i++)
		EcatDevice[DeviceNo].GrpAxisCnt[i] = 0;
	EcatDevice[DeviceNo].IsOpen = true;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return 0;

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	close(fd_dev);
	EcatDevice[DeviceNo].IsOpen = false;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_CloseDevice(uint16_t DeviceNo)
{
	int32_t ret;
	uint16_t i;

	CHECK_ECAT_DEVICE_NO(DeviceNo)
	if (!EcatDevice[DeviceNo].IsOpen)
		return 0;

	if ((!EcatDevice[DeviceNo].MpgThreadShouldStop) && (EcatDevice[DeviceNo].MpgThreadRunning)) {
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].MpgThreadShouldStop = 1;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		pthread_join(EcatDevice[DeviceNo].MpgThreadID, NULL);

	}

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	ret = irq_setup(DeviceNo, 0);
	if (ret != 0) {
		goto out_return;
	}

	SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_STOP_DEV_OP_TASK, NULL, 0);

	if ((ret = close(EcatDevice[DeviceNo].Fd)) == 0)
	{
		EcatDevice[DeviceNo].IsOpen = false;
		for (i = 0; i<MC_GROUP_NO_MAX; i++)
			EcatDevice[DeviceNo].GrpAxisCnt[i] = 0;
		ret = 0;
		goto out_return;
	}
	else
	{
		ret = ECAT_ERR_DRV_IOCTL;
		goto out_return;
	}

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceSerialNo(uint16_t DeviceNo, uint8_t *SerialNo)
{
	int32_t ret;
	ecat_read_file_return_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_DEVICE_SERIAL_NO,
							NULL, 0, &RetData, sizeof(ecat_read_file_return_t));
	printf("new ECAT_GetDeviceSerialNo\n");
	if (ret == 0) {
		memcpy(SerialNo, RetData.Data, sizeof(uint8_t) * DEVICE_SERIAL_NO_SIZE);
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceDI(uint16_t DeviceNo, uint32_t *Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	uint32_t DI = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DI, 1) != 0))
		goto out_return;
	DI = ~(DI >> 16);
	if (EcatDevice[DeviceNo].Device_Type == 801)
		DI &= 0x1FFF;
	else
		DI &= 0xFF;
	*Value = DI;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceDIBit(uint16_t DeviceNo, uint16_t BitNo, uint32_t *Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	uint32_t BitValue = 1 << BitNo;
	int32_t ret;
	uint32_t DI = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DI, 1) != 0))
		goto out_return;

	DI = ~(DI >> 16);
	if (EcatDevice[DeviceNo].Device_Type == 801)
		DI &= 0x1FFF;
	else
		DI &= 0xFF;
	DI &= BitValue;
	if (DI)
		*Value = 1;
	else
		*Value = 0;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceDO(uint16_t DeviceNo, uint32_t *Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	uint32_t DO = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DO, 1) != 0))
		goto out_return;
	DO &= 0xFFFF;
	*Value = DO;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceDOBit(uint16_t DeviceNo, uint16_t BitNo, uint32_t *Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	uint32_t BitValue = 1 << BitNo;
	int32_t ret;
	uint32_t DO = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DO, 1) != 0))
		goto out_return;
	DO &= BitValue;
	if (DO)
		*Value = 1;
	else
		*Value = 0;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceDO(uint16_t DeviceNo, uint32_t Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	uint32_t DO = Value;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	DO &= 0x1FFF;
	ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DO, 1);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceDOBit(uint16_t DeviceNo, uint16_t BitNo, uint32_t Value)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	uint32_t BitValue = 1 << BitNo;
	int32_t ret;
	uint32_t DO = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DO, 1) != 0))
		goto out_return;
	DO &= 0xFFFF;
	if (Value)
		DO |= BitValue;
	else
		DO &= ~BitValue;
	ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, GPIO_ADDR_OFFSET, &DO, 1);
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceState(uint16_t DeviceNo, uint32_t *LinkUp, uint32_t *SlavesResp, uint32_t *AlStates, uint32_t *Wc)
{
	int32_t ret;
	ecat_device_state_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_DEVICE_STATE,
							NULL, 0, &RetData, sizeof(ecat_device_state_t));
	if (ret == 0) {
		*LinkUp = RetData.LinkUp;
		*SlavesResp = RetData.SlavesResp;
		*AlStates = RetData.AlStates;
		*Wc = RetData.Wc;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveInfo(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t *Alias, uint32_t *ProductCode, uint32_t *VendorID,
	uint32_t *RevisionNo, uint32_t *SerialNo, uint8_t *AlState, uint32_t *SlaveType, char *Slavename)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_slave_info_t RetData;
	Ops.No = SlaveNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_INFO,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_slave_info_t));
	if (ret == 0) {
		*Alias = RetData.Alias;
		*ProductCode = RetData.ProductCode;
		*VendorID = RetData.VendorID;
		*RevisionNo = RetData.RevisionNo;
		*SerialNo = RetData.SerialNo;
		*AlState = RetData.AlState;
		*SlaveType = RetData.SlaveType;
		if (Slavename != NULL)
			strcpy(Slavename, RetData.Slavename);
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_UploadNetworkInfo(uint16_t DeviceNo, uint16_t NetworkInfoNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.No = NetworkInfoNo;


	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_NETWORK_INFO, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_UploadFirmware(uint16_t DeviceNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_FIRMWARE, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_DownloadNetworkInfo(uint16_t DeviceNo, uint16_t NetworkInfoNo, uint32_t *Offset, char *Data, uint32_t *DataSize, uint8_t *LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_read_file_ops_t Ops;
	ecat_read_file_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_read_file_ops_t);
	uint32_t RetSize = sizeof(ecat_read_file_return_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)

	Ops.No = NetworkInfoNo;
	if (*LastFlag == 255)
	{
		Ops.GetFileSize = 1;
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_DOWNLOAD_NETWORK_INFO, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			*DataSize = RetData.Size;
			ret = 0;
			goto out_return;
		}
	}
	else
		Ops.GetFileSize = 0;

	while (1)
	{
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_DOWNLOAD_NETWORK_INFO, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			memcpy((Data + (DataOffset)), RetData.Data, RetData.Size);
			DataOffset += RetData.Size;
			if ((RetData.LastFlag) || (*DataSize < (DataOffset + ECAT_IPC_DATA_SIZE_MAX)))
			{
				*DataSize = DataOffset;
				*Offset = RetData.Offset;
				*LastFlag = RetData.LastFlag;
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveSdoObject(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint16_t DataSize,
	uint32_t *ObjectVal, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_slave_sdo_ops_t Ops;
	ecat_slave_sdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_GET_SDO_OBJECT,
							&Ops, sizeof(ecat_slave_sdo_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret == 0)
		*ObjectVal = RetData.ObjectVal;
	else
		*AbortCode = RetData.AbortCode;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveSdoObject(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint16_t DataSize,
	uint32_t ObjectVal, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_slave_sdo_ops_t Ops;
	ecat_slave_sdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.DataSize = DataSize;
	Ops.ObjectVal = ObjectVal;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_SET_SDO_OBJECT,
							&Ops, sizeof(ecat_slave_sdo_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret != 0)
		*AbortCode = RetData.AbortCode;
	return ret;
}
/*****************************************************************************/
int32_t _SetSlaveSdoObjectEx(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint16_t DataSize,
	uint32_t ObjectVal, uint64_t TimeOut, uint32_t* AbortCode)
{
	int32_t ret;
	ecat_slave_sdo_ops_t Ops;
	ecat_slave_sdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.DataSize = DataSize;
	Ops.ObjectVal = ObjectVal;
	ret = GenericSendCommand_ex(DeviceNo, TimeOut, ECAT_IPC_CMD_SET_SDO_OBJECT,
		&Ops, sizeof(ecat_slave_sdo_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret != 0)
		* AbortCode = RetData.AbortCode;
	return ret;
}
/*****************************************************************************/
int32_t _GetSlaveSdoObjectEx(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint16_t DataSize,
	uint32_t* ObjectVal, uint64_t TimeOut, uint32_t* AbortCode)
{
	int32_t ret;
	ecat_slave_sdo_ops_t Ops;
	ecat_slave_sdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand_ex(DeviceNo, TimeOut, ECAT_IPC_CMD_GET_SDO_OBJECT,
		&Ops, sizeof(ecat_slave_sdo_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret == 0)
		* ObjectVal = RetData.ObjectVal;
	else
		*AbortCode = RetData.AbortCode;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockSdoRequestRead(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint32_t Timeout_ms)
{
	ecat_slave_sdo_ops_t Ops;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.Timeout_ms = Timeout_ms;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_SDO_REQ_READ,
							&Ops, sizeof(ecat_slave_sdo_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockSdoRequestReadState(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t DataSize, uint32_t *ObjectVal)
{
	int32_t ret;
	ecat_slave_sdo_ops_t Ops;
	ecat_slave_sdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_SDO_REQ_READ_STATE,
							&Ops, sizeof(ecat_slave_sdo_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret == 0)
		*ObjectVal = RetData.ObjectVal;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockSdoRequestWrite(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Index, uint8_t SubIndex, uint16_t DataSize, uint32_t ObjectVal, uint32_t Timeout_ms)
{
	ecat_slave_sdo_ops_t Ops;
	Ops.No = SlaveNo;
	Ops.Index = Index;
	Ops.SubIndex = SubIndex;
	Ops.DataSize = DataSize;
	Ops.ObjectVal = ObjectVal;
	Ops.Timeout_ms = Timeout_ms;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_SDO_REQ_WRITE,
							&Ops, sizeof(ecat_slave_sdo_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockSdoRequestWriteState(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t DataSize)
{
	ecat_slave_sdo_ops_t Ops;
	Ops.No = SlaveNo;
	Ops.DataSize = DataSize;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_SDO_REQ_WRITE_STATE,
							&Ops, sizeof(ecat_slave_sdo_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_StartDeviceOpTask(uint16_t DeviceNo, uint16_t NetworkInfoNo, uint8_t EnumCycleTime, uint32_t WcErrCnt)
{
	ecat_start_op_task_ops_t Ops;
	Ops.No = NetworkInfoNo;
	Ops.EnumCycleTime = EnumCycleTime;
	Ops.WcErrCnt = WcErrCnt;
	return GenericSendCommand(DeviceNo, 2000, ECAT_IPC_CMD_START_DEV_OP_TASK,
							&Ops, sizeof(ecat_slave_sdo_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_StopDeviceOpTask(uint16_t DeviceNo)
{
	return GenericSendCommand(DeviceNo, 2000, ECAT_IPC_CMD_STOP_DEV_OP_TASK,
							NULL, 0, NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveRxPdoData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, uint8_t *Data)
{
	ecat_slave_set_pdo_ops_t Ops;
	Ops.No = SlaveNo;
	Ops.OffsetByte = OffsetByte;
	Ops.DataSize = DataSize;
	memcpy(Ops.Data, Data, DataSize);
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_RXPDO_DATA,
							&Ops, sizeof(ecat_slave_set_pdo_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveRxPdoData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, uint8_t *Data)
{
	if (DataSize >= RW_PDO_DATA_SIZE_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_slave_get_pdo_ops_t Ops;
	ecat_slave_get_pdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.OffsetByte = OffsetByte;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_RXPDO_DATA,
							&Ops, sizeof(ecat_slave_get_pdo_ops_t), &RetData, sizeof(ecat_slave_get_pdo_return_t));
	if (ret == 0)
		memcpy(Data, RetData.Data, DataSize);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveRxPdoOffset(uint16_t DeviceNo, uint16_t SlaveNo,uint8_t channel, uint16_t *OffsetByte)
{
	int32_t ret;
	ecat_slave_get_pdooff_ops_t Ops;
	ecat_slave_get_pdooff_return_t RetData;
	Ops.No = SlaveNo;
	Ops.channel = channel;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_RXPDO_OFFSET,
							&Ops, sizeof(ecat_slave_get_pdooff_ops_t), &RetData, sizeof(ecat_slave_get_pdooff_return_t));
	if (ret == 0)
		*OffsetByte = RetData.offset / 8;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveRxPdoChannel(uint16_t DeviceNo, uint16_t SlaveNo, uint8_t *channel,uint8_t *bitsize)
{
	int32_t ret;
	ecat_slave_get_pdooff_ops_t Ops;
	ecat_slave_get_pdooff_return_t RetData;
	Ops.No = SlaveNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_RXPDO_CHANNEL,
							&Ops, sizeof(ecat_slave_get_pdooff_ops_t), &RetData, sizeof(ecat_slave_get_pdooff_return_t));
	if (ret == 0)
		*channel = RetData.channel;
		*bitsize = RetData.bitsize;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveTxPdoData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, uint8_t *Data)
{
	if (DataSize >= RW_PDO_DATA_SIZE_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_slave_get_pdo_ops_t Ops;
	ecat_slave_get_pdo_return_t RetData;
	Ops.No = SlaveNo;
	Ops.OffsetByte = OffsetByte;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TXPDO_DATA,
							&Ops, sizeof(ecat_slave_get_pdo_ops_t), &RetData, sizeof(ecat_slave_get_pdo_return_t));
	if (ret == 0)
		memcpy(Data, RetData.Data, DataSize);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveTxPdoOffset(uint16_t DeviceNo, uint16_t SlaveNo, uint8_t channel, uint16_t *OffsetByte)
{
	int32_t ret;
	ecat_slave_get_pdooff_ops_t Ops;
	ecat_slave_get_pdooff_return_t RetData;
	Ops.No = SlaveNo;
	Ops.channel = channel;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TXPDO_OFFSET,
							&Ops, sizeof(ecat_slave_get_pdooff_ops_t), &RetData, sizeof(ecat_slave_get_pdooff_return_t));
	if (ret == 0)
		*OffsetByte = RetData.offset / 8;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveTxPdoChannel(uint16_t DeviceNo, uint16_t SlaveNo, uint8_t *channel, uint8_t *bitsize)
{
	int32_t ret;
	ecat_slave_get_pdooff_ops_t Ops;
	ecat_slave_get_pdooff_return_t RetData;
	Ops.No = SlaveNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TXPDO_CHANNEL,
							&Ops, sizeof(ecat_slave_get_pdooff_ops_t), &RetData, sizeof(ecat_slave_get_pdooff_return_t));
	if (ret == 0)
		*channel = RetData.channel;
		*bitsize = RetData.bitsize;
	return ret;
}
/*****************************************************************************/
int32_t CommonSlaveDIO(uint16_t DeviceNo, uint32_t RunMode, uint16_t SlaveNo, uint16_t BitNo, uint32_t *Value)
{
	int32_t ret;
	ecat_slave_dio_data_t Ops;
	Ops.RunMode = RunMode;
	Ops.SlaveNo = SlaveNo;
	Ops.BitNo = BitNo;
	Ops.U32Value = *Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SLAVE_DIO,
							&Ops, sizeof(ecat_slave_dio_data_t), &Ops, sizeof(ecat_slave_dio_data_t));
	if ((RunMode != SLAVE_DIO_RM_SET_DO) && (RunMode != SLAVE_DIO_RM_SET_DO_BIT)) {
		if (ret == 0)
			*Value = Ops.U32Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDI(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t *Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_GET_DI, SlaveNo, 0, Value);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDIBit(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t *Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_GET_DI_BIT, SlaveNo, BitNo, Value);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDO(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t *Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_GET_DO, SlaveNo, 0, Value);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDOBit(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t *Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_GET_DO_BIT, SlaveNo, BitNo, Value);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveDO(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_SET_DO, SlaveNo, 0, &Value);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveDOBit(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t Value)
{
	return CommonSlaveDIO(DeviceNo, SLAVE_DIO_RM_SET_DO_BIT, SlaveNo, BitNo, &Value);
}
/*****************************************************************************/
int32_t CommonSlaveAIO(uint16_t DeviceNo, ecat_slave_aio_data_t *Ops)
{
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SLAVE_AIO,
							Ops, sizeof(ecat_slave_aio_data_t), Ops, sizeof(ecat_slave_aio_data_t));
}
/*****************************************************************************/
int32_t ECAT_SetSlaveAoProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t Range)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_SET_AO_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	Ops.Range = Range;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAoProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t *Range)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AO_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Range = Ops.Range;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveAoRawData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, int16_t Data)
{
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_SET_AO_RAW_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	Ops.RawData = Data;
	return CommonSlaveAIO(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAoRawData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, int16_t *Data)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AO_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Data = Ops.RawData;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveAoVoltData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, double Data)
{
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_SET_AO_VOLT_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	Ops.VoltData = Data;
	return CommonSlaveAIO(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAoVoltData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, double *Data)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AO_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Data = Ops.VoltData;
	return ret;
}
/*****************************************************************************/
int32_t CommonSlaveEnc(uint16_t DeviceNo, ecat_slave_enc_data_t *Ops)
{
	return GenericSendCommand(DeviceNo, 200, ECAT_IPC_CMD_SLAVE_ENC,
							Ops, sizeof(ecat_slave_enc_data_t), Ops, sizeof(ecat_slave_enc_data_t));
}
/*****************************************************************************/
int32_t ECAT_SetSlaveEncProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Mode, uint8_t InvertCnt, uint8_t LPF)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_ENC_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.Arg[0] = Mode;
	Ops.Arg[1] = InvertCnt;
	Ops.Arg[2] = LPF;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t *Mode, uint8_t *InvertCnt, uint8_t *LPF)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0)
	{
		*Mode = Ops.Arg[0];
		*InvertCnt = Ops.Arg[1];
		*LPF = Ops.Arg[2];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncCount(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t *Cnt)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0)
		*Cnt = Ops.I32Value[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_ResetSlaveEncCount(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_RESET_ENC_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveEncCount(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t Value)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_ENC_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.I32Value[0] = Value;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveEncIdxLatchProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Enable)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_ENC_IDX_LATCH_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.Arg[0] = Enable;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncIdxLatchProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t *Enable)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_IDX_LATCH_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0) {
		*Enable = Ops.Arg[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncIdxLatchCnt(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t *Cnt)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_IDX_LATCH_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0)
		*Cnt = Ops.I32Value[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_ResetSlaveEncIdxLatchCnt(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_RESET_ENC_IDX_LATCH_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveEncExtLatchProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Enable, uint8_t Mode, uint8_t Logic)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_ENC_EXT_LATCH_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.Arg[0] = Enable;
	Ops.Arg[1] = Mode;
	Ops.Arg[2] = Logic;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncExtLatchProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t *Enable, uint8_t *Mode, uint8_t *Logic)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_EXT_LATCH_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0) {
		*Enable = Ops.Arg[0];
		*Mode = Ops.Arg[1];
		*Logic = Ops.Arg[2];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncExtLatchCnt(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t* Cnt_Rising, int32_t* Cnt_Falling)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_ENC_EXT_LATCH_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0)
	{
		*Cnt_Rising = Ops.I32Value[0];
		*Cnt_Falling = Ops.I32Value[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_ResetSlaveEncExtLatchCnt(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_RESET_ENC_EXT_LATCH_CNT;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveCmpTrigProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint16_t PulseWidth)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_CMP_TRIG_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.U16Value = PulseWidth;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveCmpTrigProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint16_t *PulseWidth)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_CMP_TRIG_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0) {
		*PulseWidth = Ops.U16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveCmpTrigData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t CmpData)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_CMP_TRIG_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.I32Value[0] = CmpData;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveContCmpTrigData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t Start, uint32_t Interval, uint8_t Dir)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_CONT_CMP_TRIG_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.I32Value[0] = Start;
	Ops.U32Value = Interval;
	Ops.Arg[0] = Dir;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_McInit(uint16_t DeviceNo, uint16_t SlaveNo[], uint16_t SubAxisNo[], uint16_t AxisCount)
{
	if (AxisCount > MC_AXIS_NO_MAX)
		return ECAT_ERR_MC_AXIS_CNT_EXCEEDED;
	int32_t ret;
	ecat_mc_init_ops_t Ops;
	uint16_t i;
	memcpy(Ops.SlaveNo, SlaveNo, sizeof(uint16_t) * AxisCount);
	memcpy(Ops.SubAxisNo, SubAxisNo, sizeof(uint16_t) * AxisCount);
	Ops.AxisCount = AxisCount;
	ret =  GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_INIT,
							&Ops, sizeof(ecat_mc_init_ops_t), NULL, 0);
	if (ret == 0) 	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		memcpy(EcatDevice[DeviceNo].McSlaveNo, SlaveNo, sizeof(uint16_t) * AxisCount);
		memcpy(EcatDevice[DeviceNo].McSubAxisNo, SubAxisNo, sizeof(uint16_t) * AxisCount);
		EcatDevice[DeviceNo].McAxisCnt = AxisCount;
		for (i = 0; i < MC_AXIS_NO_MAX; i++)
			EcatDevice[DeviceNo].AxisPPU[i] = 1;
		for (i = 0; i < MC_GROUP_NO_MAX; i++)
			EcatDevice[DeviceNo].GrpAxisCnt[i] = 0;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisServoOn(uint16_t DeviceNo, uint16_t AxisNo, uint16_t State)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = State;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_SERVO_ON,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetAxisPPU(uint16_t DeviceNo, uint16_t AxisNo, double PPU)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = PPU;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_SET_AXIS_PPU,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	if (ret == 0)
		EcatDevice[DeviceNo].AxisPPU[AxisNo] = PPU;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisPPU(uint16_t DeviceNo, uint16_t AxisNo, double *PPU)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_PPU,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*PPU = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisEncoderPPR(uint16_t DeviceNo, uint16_t AxisNo, uint32_t PPR)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data = PPR;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ENCODER_PPR,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisEncoderPPR(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *PPR)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ENCODER_PPR,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*PPR = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisMotorPPR(uint16_t DeviceNo, uint16_t AxisNo, uint32_t PPR)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data = PPR;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_MOTOR_PPR,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisMotorPPR(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *PPR)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_MOTOR_PPR,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*PPR = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeMethod(uint16_t DeviceNo, uint16_t AxisNo, int32_t Method)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.I32Data = Method;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_METHOD,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeMethod(uint16_t DeviceNo, uint16_t AxisNo, int32_t *Method)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_METHOD,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Method = RetData.I32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeSpeed(uint16_t DeviceNo, uint16_t AxisNo, double SeachSw, double SeachZr)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = SeachSw;
	Ops.DoubleData[1] = SeachZr;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_SPEED,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeSpeed(uint16_t DeviceNo, uint16_t AxisNo, double *SeachSw, double *SeachZr)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_SPEED,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*SeachSw = RetData.DoubleValue[0];
		*SeachZr = RetData.DoubleValue[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeAcc(uint16_t DeviceNo, uint16_t AxisNo, double Acc)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Acc;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_ACC,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeAcc(uint16_t DeviceNo, uint16_t AxisNo, double *Acc)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_ACC,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Acc = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeOffset(uint16_t DeviceNo, uint16_t AxisNo, double Offset)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Offset;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_OFFSET,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeOffset(uint16_t DeviceNo, uint16_t AxisNo, double *Offset)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_OFFSET,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Offset = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccTime(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Time_ms)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Time_ms;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ACC_TIME,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccTime(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Time_ms)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACC_TIME,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Time_ms = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccDecType(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Type)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Type;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ACC_DEC_TYPE,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccDecType(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Type)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACC_DEC_TYPE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Type = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetVirtualAxisEncoder(uint16_t DeviceNo, uint16_t AxisNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Enable)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = SlaveNo;
	Ops.U16Data[1] = EncNo;
	Ops.U32Data = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_VIRTUAL_AXIS_ENCODER,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetVirtualAxisEncoder(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *SlaveNo, uint16_t *EncNo, uint8_t *Enable)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_VIRTUAL_AXIS_ENCODER,
							&Ops, sizeof(ecat_mc_axis_ops_t), &Ops, sizeof(ecat_mc_axis_ops_t));
	if (ret == 0) {
		*SlaveNo = Ops.U16Data[0];
		*EncNo = Ops.U16Data[1];
		*Enable = Ops.U32Data;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisTouchProbeProperty(uint16_t DeviceNo, uint16_t AxisNo, uint16_t ProbeNo, uint8_t Enable, uint8_t Logic)
{
	ecat_mc_axis_touch_probe_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.ProbeNo = ProbeNo;
	Ops.Enable = Enable;
	Ops.Logic = Logic;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_TOUCH_PROBE,
							&Ops, sizeof(ecat_mc_axis_touch_probe_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisTouchProbeProperty(uint16_t DeviceNo, uint16_t AxisNo, uint16_t ProbeNo, uint8_t *Enable, uint8_t *Logic)
{
	int32_t ret;
	ecat_mc_axis_touch_probe_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.ProbeNo = ProbeNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_TOUCH_PROBE,
							&Ops, sizeof(ecat_mc_axis_touch_probe_ops_t), &Ops, sizeof(ecat_mc_axis_touch_probe_ops_t));
	if (ret == 0) {
		*Enable = Ops.Enable;
		*Logic = Ops.Logic;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisTouchProbeValue(uint16_t DeviceNo, uint16_t AxisNo, uint16_t ProbeNo, double *Value)
{
	int32_t ret;
	ecat_mc_axis_touch_probe_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.ProbeNo = ProbeNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_TOUCH_PROBE_VALUE,
							&Ops, sizeof(ecat_mc_axis_touch_probe_ops_t), &Ops, sizeof(ecat_mc_axis_touch_probe_ops_t));
	if (ret == 0) {
		*Value = Ops.Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisState(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *State)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_STATE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(State + i) = RetData.U32Value32[i];
		} else
			*State = RetData.U32Value32[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisMotionState(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *State)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_MOTION_STATE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0)
		*State = RetData.U32Value32[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisLastError(uint16_t DeviceNo, uint16_t AxisNo, int32_t *Error)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_LAST_ERROR,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Error + i) = RetData.I32Value32[i];
		} else
			*Error = RetData.I32Value32[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDriveError(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Error)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DRIVE_ERROR,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Error + i) = RetData.U16Value32[i];
		} else
			*Error = RetData.U16Value32[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualPos(uint16_t DeviceNo, uint16_t AxisNo, double *Pos)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACT_POS,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Pos + i) = RetData.DoubleValue_multi[i];
		} else
			*Pos = RetData.DoubleValue_multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisCommandPos(uint16_t DeviceNo, uint16_t AxisNo, double *Pos)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_CMD_POS,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Pos + i) = RetData.DoubleValue_multi[i];
		} else
			*Pos = RetData.DoubleValue_multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualVel(uint16_t DeviceNo, uint16_t AxisNo, double *Vel)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACT_VEL,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Vel + i) = RetData.DoubleValue_multi[i];
		} else
			*Vel = RetData.DoubleValue_multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *DI)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(DI + i) = RetData.U32Value32[i];
		} else
			*DI = RetData.U32Value32[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetProfileData(uint16_t DeviceNo, uint16_t ProfileNo, double *Data, uint16_t DataSize)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	CHECK_MC_PROFILE_NO(ProfileNo)
	if (DataSize > MC_PROFILE_DATA_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_mc_rw_profile_data_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_mc_rw_profile_data_ops_t);

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.ProfileNo = ProfileNo;

	for (Ops.Offset = 0; Ops.Offset<DataSize; Ops.Offset += ECAT_IPC_PROFILE_DATA_SIZE_MAX)
	{
		Ops.Size = MIN(ECAT_IPC_PROFILE_DATA_SIZE_MAX, DataSize - Ops.Offset);
		memcpy(Ops.Data, (Data + Ops.Offset), sizeof(double)*Ops.Size);
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_SET_PROFILE_DATA, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		ret = GetReturnData(&EcatDevice[DeviceNo], NULL, 0);
		if (ret != 0)
			break;
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetProfileData(uint16_t DeviceNo, uint16_t ProfileNo, double *Data, uint16_t DataSize)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	CHECK_MC_PROFILE_NO(ProfileNo)
	if (DataSize > MC_PROFILE_DATA_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_mc_rw_profile_data_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_mc_rw_profile_data_ops_t);

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.ProfileNo = ProfileNo;

	for (Ops.Offset = 0; Ops.Offset<DataSize; Ops.Offset += ECAT_IPC_PROFILE_DATA_SIZE_MAX)
	{
		Ops.Size = MIN(ECAT_IPC_PROFILE_DATA_SIZE_MAX, DataSize - Ops.Offset);
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_GET_PROFILE_DATA, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&Ops, OpsSize);

		if (ret != 0)
			break;
		else
			memcpy((Data + Ops.Offset), Ops.Data, sizeof(double)*Ops.Size);

	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetProfileCSV(uint16_t DeviceNo, uint16_t ProfileNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.No = ProfileNo;


	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_SET_PROFILE_CSV, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
//MartinK 2021_03_26
//
//  - The PLC can not download all the data of the csv file within a task cycle
//    The task cycle time is set by the user and it may occurr that the task cycle 
//    is greater than the timeout set inside the ECAT-M801
//  - This API allows the PLC to close the CSV file inside the ECAT-M801 even if 
//    not all the data of the file has been sent. In the next PLC cycle the same file
//    will be openend and the remaining data appended to the end position of the file.
int32_t ECAT_McSetProfileCSV_PLC(uint16_t DeviceNo, uint16_t ProfileNo, uint32_t Offset, char* pData, uint32_t DataSize, uint8_t FileFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	CHECK_CSV_DATA_SIZE(DataSize)
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		Ops.No = ProfileNo;


	Ops.Size = DataSize;
	Ops.Offset = DataOffset;
	Ops.LastFlag = FileFlag;
	memcpy(Ops.Data, pData, DataSize);

	if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_SET_PROFILE_CSV, (uint32_t*)& Ops, OpsSize)) < 0)
		goto out_return;

	if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)& RetData, RetSize)) < 0)
		goto out_return;
	else
	{
		DataOffset += RetData.WriteBytes;
		DataSize -= RetData.WriteBytes;
		if (DataSize == 0)
		{
			ret = 0;
			goto out_return;
		}
	}

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetProfileCSV(uint16_t DeviceNo, uint16_t ProfileNo, uint32_t *Offset, char *Data, uint32_t *DataSize, uint8_t *LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_read_file_ops_t Ops;
	ecat_read_file_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_read_file_ops_t);
	uint32_t RetSize = sizeof(ecat_read_file_return_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)

	Ops.No = ProfileNo;
	if (*LastFlag == 255)
	{
		Ops.GetFileSize = 1;
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_GET_PROFILE_CSV, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			*DataSize = RetData.Size;
			ret = 0;
			goto out_return;
		}
	}
	else
		Ops.GetFileSize = 0;

	while (1)
	{
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_GET_PROFILE_CSV, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			memcpy((Data + (DataOffset)), RetData.Data, RetData.Size);
			DataOffset += RetData.Size;
			if ((RetData.LastFlag) || (*DataSize < (DataOffset + ECAT_IPC_DATA_SIZE_MAX)))
			{
				*DataSize = DataOffset;
				*Offset = RetData.Offset;
				*LastFlag = RetData.LastFlag;
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetEcamTable(uint16_t DeviceNo, uint16_t TableNo, double *Data, uint16_t DataSize)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	CHECK_MC_ECAM_TABLE_NO(TableNo)
	if (DataSize > MC_ECAM_TABLE_DATA_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_mc_rw_double_data_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_mc_rw_double_data_ops_t);

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.No = TableNo;

	for (Ops.Offset = 0; Ops.Offset < DataSize; Ops.Offset += ECAT_IPC_RW_DOUBLE_DATA_SIZE_MAX)
	{
		Ops.Size = MIN(ECAT_IPC_RW_DOUBLE_DATA_SIZE_MAX, DataSize - Ops.Offset);
		memcpy(Ops.Data, (Data + Ops.Offset), sizeof(double)*Ops.Size);
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_SET_ECAM_TABLE, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		ret = GetReturnData(&EcatDevice[DeviceNo], NULL, 0);
		if (ret != 0)
			break;
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetEcamTable(uint16_t DeviceNo, uint16_t TableNo, double *Data, uint16_t DataSize)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	CHECK_MC_ECAM_TABLE_NO(TableNo)
	if (DataSize > MC_ECAM_TABLE_DATA_MAX)
		return ECAT_ERR_INVALID_DATA_SIZE;
	int32_t ret;
	ecat_mc_rw_double_data_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_mc_rw_double_data_ops_t);

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		Ops.No = TableNo;

	for (Ops.Offset = 0; Ops.Offset < DataSize; Ops.Offset += ECAT_IPC_RW_DOUBLE_DATA_SIZE_MAX)
	{
		Ops.Size = MIN(ECAT_IPC_RW_DOUBLE_DATA_SIZE_MAX, DataSize - Ops.Offset);
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_GET_ECAM_TABLE, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&Ops, OpsSize);

		if (ret != 0)
			break;
		else
			memcpy((Data + Ops.Offset), Ops.Data, sizeof(double)*Ops.Size);
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McConfigEcamTable(uint16_t DeviceNo, uint16_t TableNo, uint8_t SlaveAbs)
{
	ecat_mc_config_ecam_ops_t Ops;
	Ops.No = TableNo;
	Ops.SlaveAbs = SlaveAbs;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_CONFIG_ECAM_TABLE,
							&Ops, sizeof(ecat_mc_config_ecam_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisHome(uint16_t DeviceNo, uint16_t AxisNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_HOME,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisErrorReset(uint16_t DeviceNo, uint16_t AxisNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_ERROR_RESET,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisStop(uint16_t DeviceNo, uint16_t AxisNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_STOP,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisQuickStop(uint16_t DeviceNo, uint16_t AxisNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_QUICK_STOP,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveAbs(uint16_t DeviceNo, uint16_t AxisNo, double Pos, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_ABS,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveRel(uint16_t DeviceNo, uint16_t AxisNo, double Pos, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_REL,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisChangePos(uint16_t DeviceNo, uint16_t AxisNo, double Pos)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_CHANGE_POS,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisChangeVel(uint16_t DeviceNo, uint16_t AxisNo, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_CHANGE_VEL,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveVel(uint16_t DeviceNo, uint16_t AxisNo, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_VEL,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveVelByPos(uint16_t DeviceNo, uint16_t AxisNo, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_VEL_BY_POS,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisGearIn(uint16_t DeviceNo, uint16_t MasterNo, uint16_t SlaveNo, int32_t RatioNum, uint32_t RationDen, uint16_t SyncSource)
{
	ecat_mc_axis_ops_t Ops;
	Ops.MasterNo = MasterNo;
	Ops.AxisNo = SlaveNo;
	Ops.I32Data = RatioNum;
	Ops.U32Data = RationDen;
	Ops.U16Data[0] = SyncSource;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GEAR_IN,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisGearOut(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Stop)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = SlaveNo;
	switch (Stop)
	{
	case 1:
		return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_STOP,
								&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	case 2:
		return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_QUICK_STOP,
								&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	default:
		return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GEAR_OUT,
								&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	}
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveProfile(uint16_t DeviceNo, uint16_t AxisNo, uint16_t ProfileNo, uint16_t TotalStep)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = ProfileNo;
	Ops.U16Data[1] = TotalStep;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_PROFILE,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveProfileCSV(uint16_t DeviceNo, uint16_t AxisNo, uint16_t ProfileNo)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = ProfileNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_PROFILE_CSV,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisCamIn(uint16_t DeviceNo, uint16_t MasterNo, uint16_t SlaveNo, uint16_t TableNo, uint16_t SyncSource,
	double MasterInterval, double SlaveScaling)
{
	ecat_mc_axis_ops_t Ops;
	Ops.MasterNo = MasterNo;
	Ops.AxisNo = SlaveNo;
	Ops.U16Data[0] = TableNo;
	Ops.U16Data[1] = SyncSource;
	Ops.DoubleData[0] = MasterInterval;
	Ops.DoubleData[1] = SlaveScaling;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_CAM_IN,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisCamOut(uint16_t DeviceNo, uint16_t SlaveNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = SlaveNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_CAM_OUT,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisCamPhaseShift(uint16_t DeviceNo, uint16_t SlaveNo, double PhaseShift)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = SlaveNo;
	Ops.DoubleData[0] = PhaseShift;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_CAM_PHASE_SHIFT,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisTangentInGroup(uint16_t DeviceNo, uint16_t AxisNo, uint16_t GroupNo, double Angle, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = GroupNo;
	Ops.DoubleData[0] = Angle;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_TANGENT_IN_GROUP,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAddAxisToGroup(uint16_t DeviceNo, uint16_t GroupNo, uint16_t AxisNo)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_ADD_AXIS,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	if (ret == 0) {
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]++;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McRemoveAxisFromGroup(uint16_t DeviceNo, uint16_t GroupNo, uint16_t AxisNo)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_REMOVE_AXIS,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	if (ret == 0) {
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]--;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McUngroupAllAxes(uint16_t DeviceNo, uint16_t GroupNo)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_UNGROUP,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	if (ret == 0) {
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].GrpAxisCnt[GroupNo] = 0;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupCmdMode(uint16_t DeviceNo, uint16_t GroupNo, uint16_t CmdMode)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = CmdMode;
	Ops.U16Data[1] = 0; //change mode immediately
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_CMD_MODE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupCmdMode(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *CmdMode)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_CMD_MODE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*CmdMode = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupState(uint16_t DeviceNo, uint16_t GroupNo, uint32_t *State)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_STATE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*State = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupCmdBuffer(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *Buffer)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_CMD_BUFFER,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Buffer = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupAccTime(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Time_ms)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Time_ms;
	Ops.U16Data[1] = 0; //change mode immediately
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_ACC_TIME,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAccTime(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *Time_ms)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_ACC_TIME,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Time_ms = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupAccDecType(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Type)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Type;
	Ops.U16Data[1] = 0; //change mode immediately
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_ACC_DEC_TYPE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAccDecType(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *Type)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_ACC_DEC_TYPE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Type = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupStop(uint16_t DeviceNo, uint16_t GroupNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = GroupNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_STOP,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupQuickStop(uint16_t DeviceNo, uint16_t GroupNo)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = GroupNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_QUICK_STOP,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t SendMcGroupMoveLine(uint16_t DeviceNo, uint16_t GroupNo, uint8_t Mode, bool AbsMove, double Vel, double Time,
	double Pos[], double pvt_vel[])
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.Vel = Vel;
	Ops.Time = Time;
	Ops.AbsMove = AbsMove;
	Ops.Mode = Mode;
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.DoubleData, Pos, sizeof(double)*EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	if (Mode == MC_LINE_MODE_PVT)
		memcpy(Ops.pvt_vel, pvt_vel, sizeof(double)*EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_LINE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Vel)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PV, true, Vel, 0, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_PT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PT, true, 0, Time, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_PTexT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Time, double exTime)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PTT, true, exTime, Time, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_PVT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Vel[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PVT, true, 0, Time, Pos, Vel);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_PPT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PPT, true, 0, Time, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_PT_A(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PTA, true, 0, Time, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineRel(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Vel)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PV, false, Vel, 0, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineRel_PT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PT, false, 0, Time, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineRel_PVT(uint16_t DeviceNo, uint16_t GroupNo, double Pos[], double Vel[], double Time)
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_PVT, false, 0, Time, Pos, Vel);
}
/*****************************************************************************/
void Vector_Cross(uint16_t Dim, double *OutVector, double *Vect1, double *Vect2)
{
	uint16_t i, j = 1, k = 2;

	for (i = 0; i<Dim; i++)
	{
		OutVector[i] = Vect1[j] * Vect2[k] - Vect1[k] * Vect2[j];
		j++; k++;
		if (j == Dim) j = 0;
		if (k == Dim) k = 0;
	}
}
/*****************************************************************************/
double Vector_Dot(uint16_t Dim, double *Vect1, double *Vect2)
{
	uint16_t i;
	double Value = 0.0;

	for (i = 0; i<Dim; i++)
		Value += Vect1[i] * Vect2[i];
	return Value;
}
/*****************************************************************************/
int32_t FindNV(double *SP, double *CP, double *EP, double *NV)
{
	int i;
	double v1[3], v2[3];
	double MNV = 0;
	for (i = 0; i < 3; i++)
	{
		v1[i] = SP[i] - CP[i];
		v2[i] = EP[i] - CP[i];
	}

	Vector_Cross(3, NV, v1, v2);

	for (i = 0; i < 3; i++)
		MNV += (NV[i] * NV[i]);

	if (MNV == 0.0)
		return -1;

	MNV = sqrt(MNV);
	for (i = 0; i < 3; i++)
		NV[i] = NV[i] / MNV;

	return 0;
}
/*****************************************************************************/
int32_t CheckNormalVector(double *Vec)
{
	int i;
	double MVec = 0;
	for (i = 0; i < 3; i++)
		MVec += (Vec[i] * Vec[i]);

	if (MVec == 0.0)
		return -1;

	MVec = sqrt(MVec);
	for (i = 0; i < 3; i++)
		Vec[i] = Vec[i] / MVec;

	return 0;
}
/*****************************************************************************/
int32_t FindCP_2D(double *p1, double *p2, double *p3, double *CP)
{
	double DeltaX21, DeltaY21, DeltaX32, DeltaY32;
	double MX1, MY1, MX2, MY2, K1, K2;
	bool	K1zero, K2zero, K1inf, K2inf;

	DeltaX21 = p2[0] - p1[0];	//(X2-X1)
	DeltaY21 = p2[1] - p1[1];	//(Y2-Y1)
	DeltaX32 = p3[0] - p2[0];	//(X3-X2)
	DeltaY32 = p3[1] - p2[1];	//(Y3-Y2)

	MX1 = (p1[0] + p2[0]) / 2;	//(X1+X2) / 2
	MY1 = (p1[1] + p2[1]) / 2;	//(Y1+Y2) / 2
	MX2 = (p2[0] + p3[0]) / 2;	//(X2+X3) / 2
	MY2 = (p2[1] + p3[1]) / 2;	//(Y2+Y3) / 2


	K1zero = (DeltaX21 <= MC_DOUBLE_EQUAL_ZERO) && (DeltaX21 >= -MC_DOUBLE_EQUAL_ZERO);
	K1inf = (DeltaY21 <= MC_DOUBLE_EQUAL_ZERO) && (DeltaY21 >= -MC_DOUBLE_EQUAL_ZERO);
	K2zero = (DeltaX32 <= MC_DOUBLE_EQUAL_ZERO) && (DeltaX32 >= -MC_DOUBLE_EQUAL_ZERO);
	K2inf = (DeltaY32 <= MC_DOUBLE_EQUAL_ZERO) && (DeltaY32 >= -MC_DOUBLE_EQUAL_ZERO);

	if ((K1zero && K2zero) || (K1inf && K2inf))
		return -1;
	else if (K1zero || K2zero)
	{
		if (K1zero)
		{
			K2 = -DeltaX32 / DeltaY32;
			CP[1] = MY1;
			CP[0] = ((CP[1] - MY2) / K2) + MX2;
		}
		else
		{
			K1 = -DeltaX21 / DeltaY21;
			CP[1] = MY2;
			CP[0] = ((CP[1] - MY1) / K1) + MX1;
		}
	}
	else if (K1inf || K2inf)
	{
		if (K1inf)
		{
			K2 = -DeltaX32 / DeltaY32;
			CP[0] = MX1;
			CP[1] = (K2 * (CP[0] - MX2)) + MY2;
		}
		else
		{
			K1 = -DeltaX21 / DeltaY21;
			CP[0] = MX2;
			CP[1] = (K1 * (CP[0] - MX1)) + MY1;
		}
	}
	else
	{
		double K1MX1, K2MX2;
		K1 = -DeltaX21 / DeltaY21;
		K2 = -DeltaX32 / DeltaY32;
		K1MX1 = K1 * MX1;
		K2MX2 = K2 * MX2;
		CP[0] = (K1MX1 - K2MX2 - MY1 + MY2) / (K1 - K2);
		CP[1] = (K1 * (CP[0] - MX1)) + MY1;
	}
	return 0;
}
/*****************************************************************************/
int32_t FindCP_3D(double *p1, double *p2, double *p3, double *CP, double *NV)
{
	double x1, y1, z1;
	double x2, y2, z2;
	double x3, y3, z3;
	double a1, b1, c1, d1;
	double a2, b2, c2, d2;
	double a3, b3, c3, d3;
	double A[3][3];
	double MNV = 0;
	int i;

	x1 = p1[0];
	y1 = p1[1];
	z1 = p1[2];
	x2 = p2[0];
	y2 = p2[1];
	z2 = p2[2];
	x3 = p3[0];
	y3 = p3[1];
	z3 = p3[2];


	// plane 1 ()
	a1 = (x2 - x1);
	b1 = (y2 - y1);
	c1 = (z2 - z1);
	d1 = (x2*x2 - x1*x1 + y2*y2 - y1*y1 + z2*z2 - z1*z1) / 2;
	// plane 2
	a2 = (x3 - x2);
	b2 = (y3 - y2);
	c2 = (z3 - z2);
	d2 = (x3*x3 - x2*x2 + y3*y3 - y2*y2 + z3*z3 - z2*z2) / 2;

	// plane 3
	a3 = (b1*c2 - b2*c1);  //cross product
	b3 = (c1*a2 - c2*a1);
	c3 = (a1*b2 - a2*b1);
	d3 = a3*x1 + b3*y1 + c3*z1;

	A[0][0] = a1;    	A[0][1] = b1;   	A[0][2] = c1;
	A[1][0] = a2;    	A[1][1] = b2;   	A[1][2] = c2;
	A[2][0] = a3;    	A[2][1] = b3;   	A[2][2] = c3;

	if (InverseMatrix((double *)A) < 0)
		return -1; // matrix inversion error!


				   /// cal center point
	CP[0] = A[0][0] * d1 + A[0][1] * d2 + A[0][2] * d3;
	CP[1] = A[1][0] * d1 + A[1][1] * d2 + A[1][2] * d3;
	CP[2] = A[2][0] * d1 + A[2][1] * d2 + A[2][2] * d3;

	NV[0] = a3;
	NV[1] = b3;
	NV[2] = c3;
	for (i = 0; i < 3; i++)
		MNV += (NV[i] * NV[i]);
	MNV = sqrt(MNV);
	for (i = 0; i < 3; i++)
		NV[i] = NV[i] / MNV;

	return 0;
}
/*****************************************************************************/
int32_t InverseMatrix(double *a)
{
	int16_t n = 3;
	int16_t is[3], js[3], i, j, k, l, u, v; /// limited usage for 3x3 matrix
	double d, p;

	for (k = 0; k <= n - 1; k++)
	{
		d = 0.0;
		for (i = k; i <= n - 1; i++)
			for (j = k; j <= n - 1; j++)
			{
				l = i*n + j;
				p = fabs(a[l]);
				if (p>d)
				{
					d = p;
					is[k] = i;
					js[k] = j;
				}
			}
		if (d + 1.0 == 1.0)
			return -1;

		if (is[k] != k)
		{
			for (j = 0; j <= n - 1; j++)
			{
				u = k*n + j;
				v = is[k] * n + j;
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		}
		if (js[k] != k)
		{
			for (i = 0; i <= n - 1; i++)
			{
				u = i*n + k;
				v = i*n + js[k];
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		}
		l = k*n + k;
		a[l] = 1.0 / a[l];
		for (j = 0; j <= n - 1; j++)
		{
			if (j != k)
			{
				u = k*n + j;
				a[u] = a[u] * a[l];
			}
		}
		for (i = 0; i <= n - 1; i++)
		{
			if (i != k)
			{
				for (j = 0; j <= n - 1; j++)
				{
					if (j != k)
					{
						u = i*n + j;
						a[u] = a[u] - a[i*n + k] * a[k*n + j];
					}
				}
			}
		}
		for (i = 0; i <= n - 1; i++)
		{
			if (i != k)
			{
				u = i*n + k;
				a[u] = -a[u] * a[l];
			}
		}

	}

	for (k = n - 1; k >= 0; k--)
	{
		if (js[k] != k)
		{
			for (j = 0; j <= n - 1; j++)
			{
				u = k*n + j;
				v = js[k] * n + j;
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		}
		if (is[k] != k)
		{
			for (i = 0; i <= n - 1; i++)
			{
				u = i*n + k;
				v = i*n + is[k];
				p = a[u];
				a[u] = a[v];
				a[v] = p;
			}
		}
	}
	return 0;
}
/*****************************************************************************/
int32_t SendMcGroupMoveCircHelix(uint16_t DeviceNo, uint16_t GroupNo, uint8_t CircMode, uint8_t CircDim, bool AbsMove, uint8_t MoveHelixType,
	double Vel, double Angle, uint8_t Dir, double Pitch, double AuxPos[], double EndPos[], double NV[], double EndRadius)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret = 0;
	uint16_t i;
	ecat_mc_group_ops_t Ops;
	ecat_ipc_base_ops_t Ops2;
	ecat_mc_group_ops_t RetData;
	double R, EP_Ridus;
	double x_UnitVector[3] = { 0,0,0 }, y_UnitVector[3] = { 0,0,0 };  //x' y' UnitVector, Shift rotating center to c ,
																	  //where c is the center of the circle, translate to the new coordinate system o' x' y' z',
	double x_PjtValue, y_PjtValue;  				 //x' y' projection value
	double SP_ABS[3] = { 0,0,0 };
	double CP_ABS[3] = { 0,0,0 };
	double EP_ABS[3] = { 0,0,0 };
	double SP_Rel_CP[3] = { 0,0,0 };
	double EP_Rel_CP[3] = { 0,0,0 };


	if (CircMode != MC_CIRC_MODE_CP_ANGLE) {
		Ops2.No = GroupNo;
		ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXIS_CMD_POS,
								&Ops2, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_group_ops_t));
		if (ret != 0)
			goto out_return;
		else
			memcpy(SP_ABS, RetData.DoubleData, sizeof(SP_ABS));
	}

	//Calculate radius
	R = 0;
	EP_Ridus = 0;
	if (CircMode == MC_CIRC_MODE_BP_EP)
	{

		for (i = 0; i < CircDim; i++)
		{
			if (!AbsMove)
			{
				AuxPos[i] += SP_ABS[i];
				EndPos[i] += SP_ABS[i];
			}
		}


		if (CircDim == 2)
		{
			if (FindCP_2D(SP_ABS, AuxPos, EndPos, CP_ABS) < 0)
			{
				ret = ECAT_ERR_MC_INVALID_PARAM;
				goto out_return;
			}
		}
		else
		{
			if (FindCP_3D(SP_ABS, AuxPos, EndPos, CP_ABS, NV) < 0)
			{
				ret = ECAT_ERR_MC_INVALID_PARAM;
				goto out_return;
			}
		}

		for (i = 0; i < CircDim; i++)
		{
			SP_Rel_CP[i] = (SP_ABS[i] - CP_ABS[i]);
			EP_Rel_CP[i] = EndPos[i] - CP_ABS[i];
			R = R + (SP_Rel_CP[i] * SP_Rel_CP[i]);

			if (AbsMove)
				AuxPos[i] = CP_ABS[i];
			else
				AuxPos[i] = -SP_Rel_CP[i];
		}

		R = sqrt(R);
		if (R <= MC_DOUBLE_EQUAL_ZERO)
		{
			ret = ECAT_ERR_MC_INVALID_RADIUS;
			goto out_return;
		}
	}
	else if (CircMode == MC_CIRC_MODE_CP_EP)
	{
		for (i = 0; i < CircDim; i++)
		{
			if (AbsMove)
			{
				SP_Rel_CP[i] = (SP_ABS[i] - AuxPos[i]);
				CP_ABS[i] = AuxPos[i];
				EP_ABS[i] = EndPos[i];
			}
			else
			{
				SP_Rel_CP[i] = -AuxPos[i];
				CP_ABS[i] = (SP_ABS[i] + AuxPos[i]);
				EP_ABS[i] = SP_ABS[i] + EndPos[i];
			}
			R = R + (SP_Rel_CP[i] * SP_Rel_CP[i]);

			EP_Rel_CP[i] = EP_ABS[i] - CP_ABS[i];
			EP_Ridus = EP_Ridus + (EP_Rel_CP[i] * EP_Rel_CP[i]);


		}

		if (CircDim == 3)
		{
			if (FindNV(SP_ABS, CP_ABS, EP_ABS, NV) < 0)
			{
				ret = ECAT_ERR_MC_INVALID_END_POS;
				goto out_return;
			}
		}

		R = sqrt(R);
		if (R <= MC_DOUBLE_EQUAL_ZERO)
		{
			ret = ECAT_ERR_MC_INVALID_RADIUS;
			goto out_return;
		}

		EP_Ridus = sqrt(EP_Ridus);
		if (fabs(R - EP_Ridus) >= 0.01)
		{
			ret = ECAT_ERR_MC_INVALID_END_POS;
			goto out_return;
		}
	}


	//Define Normal vector
	if (CircDim == 2)
	{
		NV[0] = 0.0;
		NV[1] = 0.0;
		NV[2] = 1.0;
	}
	if ((CircMode == MC_CIRC_MODE_CP_EP) || (CircMode == MC_CIRC_MODE_BP_EP))
	{

		for (i = 0; i < CircDim; i++)
			x_UnitVector[i] = SP_Rel_CP[i] / R;

		Vector_Cross(3, y_UnitVector, NV, x_UnitVector);
		x_PjtValue = Vector_Dot(3, x_UnitVector, EP_Rel_CP);
		y_PjtValue = Vector_Dot(3, y_UnitVector, EP_Rel_CP);
		Angle = atan2(y_PjtValue, x_PjtValue); // 0~180->0~pi,180~360->-pi~0

		if (Angle >= 0)
		{
			if (!Dir)
				Angle = -M_2PI + Angle;
		}
		else
		{
			if (Dir)
				Angle = M_2PI + Angle;
		}
		Angle = Angle * 180 / M_PI;
	}

	if (Angle < 0)
		Pitch = Pitch * -1;

	Ops.GroupNo = GroupNo;
	Ops.Vel = Vel;
	Ops.Angle = Angle;
	Ops.CircDim = CircDim;
	Ops.AbsMove = AbsMove;
	Ops.MoveHelixType = MoveHelixType;
	Ops.Pitch = Pitch;
	Ops.EndRadius = EndRadius;
	memcpy(Ops.DoubleData, AuxPos, sizeof(double) * 3);
	memcpy(Ops.NormalVector, NV, sizeof(Ops.NormalVector));

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_CIRC_HELIX,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);

out_return:
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularAbs(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Vel)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularAbs_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double AuxPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularAbs_CP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_EP, 2, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularAbs_BP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_BP_EP, 2, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularAbs_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double AuxPos[], double NV[])
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularAbs_CP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_EP, 3, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularAbs_BP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_BP_EP, 3, true, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularRel(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Vel)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularRel_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double AuxPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularRel_CP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_EP, 2, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircularRel_BP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_BP_EP, 2, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularRel_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double AuxPos[], double NV[])
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, Angle, 0, 0, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularRel_CP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_EP, 3, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DCircularRel_BP_EP(uint16_t DeviceNo, uint16_t GroupNo, double Vel, uint8_t Dir, double AuxPos[], double EndPos[])
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_BP_EP, 3, false, MC_GRP_NOT_MOVE_HELIX,
		Vel, 0, Dir, 0, AuxPos, EndPos, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveHelicalAbs(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Pitch, double Vel)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, true, MC_GRP_MOVE_NORMAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DHelicalAbs_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double Pitch, double AuxPos[], double NV[])
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, true, MC_GRP_MOVE_NORMAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveHelicalRel(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Pitch, double Vel)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, false, MC_GRP_MOVE_NORMAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DHelicalRel_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double Pitch, double AuxPos[], double NV[])
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, false, MC_GRP_MOVE_NORMAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveConicalHelixAbs(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Pitch, double Vel, double EndRadius)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, true, MC_GRP_MOVE_CONICAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, EndRadius);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DConicalHelixAbs_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double Pitch, double AuxPos[], double NV[], double EndRadius)
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, true, MC_GRP_MOVE_CONICAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, EndRadius);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveConicalHelixRel(uint16_t DeviceNo, uint16_t GroupNo, double Angle, double AuxPos[], double Pitch, double Vel, double EndRadius)
{
	double NV[3];
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 2, false, MC_GRP_MOVE_CONICAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, EndRadius);
}
/*****************************************************************************/
int32_t ECAT_McGroupMove3DConicalHelixRel_CP_Angle(uint16_t DeviceNo, uint16_t GroupNo, double Vel, double Angle, double Pitch, double AuxPos[], double NV[], double EndRadius)
{
	return SendMcGroupMoveCircHelix(DeviceNo, GroupNo, MC_CIRC_MODE_CP_ANGLE, 3, false, MC_GRP_MOVE_CONICAL_HELIX,
		Vel, Angle, 0, Pitch, AuxPos, NULL, NV, EndRadius);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveProfile(uint16_t DeviceNo, uint16_t GroupNo, uint16_t ProfileNo[], uint16_t TotalStep)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = TotalStep;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.ProfileNo, ProfileNo, sizeof(uint16_t)*EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_PROFILE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveProfileCSV(uint16_t DeviceNo, uint16_t GroupNo, uint16_t ProfileNo)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.ProfileNo[0] = ProfileNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_PROFILE_CSV,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveDwell(uint16_t DeviceNo, uint16_t GroupNo, uint32_t Cnt)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U32Data = Cnt;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_DWELL,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveDO(uint16_t DeviceNo, uint16_t GroupNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t Value)
{
	ecat_mc_group_move_do_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.SlaveNo = SlaveNo;
	Ops.BitNo = BitNo;
	Ops.Value = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_DO,
							&Ops, sizeof(ecat_mc_group_move_do_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveBlendingSync(uint16_t DeviceNo, uint16_t GroupNo)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_BLENDING_SYNC,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SetDeviceEncProperty(uint16_t DeviceNo, uint16_t EncNo, uint8_t Mode, uint8_t InvertCnt, uint8_t LPF)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (EncNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	encoder_config_t config;
	uint32_t offset;
	if (EncNo == 0)
		offset = ENC0_MODE_ADDR_OFFSET;
	else
		offset = ENC1_MODE_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config.Cfg, 1) != 0))
		goto out_return;
	config.Mode0 = InvertCnt;
	config.EncoderMode = Mode;
	config.LPF = LPF;
	ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config.Cfg, 1);
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceEncProperty(uint16_t DeviceNo, uint16_t EncNo, uint8_t *Mode, uint8_t *InvertCnt, uint8_t *LPF)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (EncNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	encoder_config_t config;
	uint32_t offset;
	if (EncNo == 0)
		offset = ENC0_MODE_ADDR_OFFSET;
	else
		offset = ENC1_MODE_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config.Cfg, 1) != 0))
		goto out_return;
	*InvertCnt = config.Mode0;
	*Mode = config.EncoderMode;
	*LPF = config.LPF;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceEnCount(uint16_t DeviceNo, uint16_t EncNo, int32_t *Cnt)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (EncNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t offset;
	int32_t enc_cnt = 0;
	if (EncNo == 0)
		offset = ENC0_CNT_ADDR_OFFSET;
	else
		offset = ENC1_CNT_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &enc_cnt, 1) != 0))
		goto out_return;
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &enc_cnt, 1) != 0))
		goto out_return;
	*Cnt = enc_cnt;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceEncCount(uint16_t DeviceNo, uint16_t EncNo, int32_t* Cnt)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (EncNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t offset;
	int32_t enc_cnt = 0;
	if (EncNo == 0)
		offset = ENC0_CNT_ADDR_OFFSET;
	else
		offset = ENC1_CNT_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &enc_cnt, 1) != 0))
			goto out_return;
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &enc_cnt, 1) != 0))
		goto out_return;
	*Cnt = enc_cnt;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_ResetDeviceEncCount(uint16_t DeviceNo, uint16_t EncNo)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (EncNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	encoder_config_t config;
	uint32_t offset;
	if (EncNo == 0)
		offset = ENC0_MODE_ADDR_OFFSET;
	else
		offset = ENC1_MODE_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config.Cfg, 1) != 0))
		goto out_return;
	ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config.Cfg, 1);
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceCmpTrigProperty(uint16_t DeviceNo, uint16_t CmpNo, uint32_t PulseWidth, uint8_t Source)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (CmpNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t config;
	uint32_t offset;
	if (CmpNo == 0)
		offset = ENC0_CMP_TRIG_CONFIG_ADDR_OFFSET;
	else
		offset = ENC1_CMP_TRIG_CONFIG_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config, 1) != 0))
		goto out_return;
	PulseWidth |= 0x80000000;
	config &= 0x80000000;
	config |= PulseWidth;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config, 1) != 0))
		goto out_return;

	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;
	config &= ~0x1C;
	if (Source == 0)//encoder x, y
		config &= ~0x10;
	else if (Source == 1)//encoder y, x
		config |= 0x10;
	else if (Source == 2)//encoder x, x
		config |= 0x08;
	else if (Source == 3)//encoder y, y
		config |= 0x04;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceCmpTrigProperty(uint16_t DeviceNo, uint16_t CmpNo, uint32_t *PulseWidth, uint8_t *Source)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (CmpNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t config;
	uint32_t offset;
	if (CmpNo == 0)
		offset = ENC0_CMP_TRIG_CONFIG_ADDR_OFFSET;
	else
		offset = ENC1_CMP_TRIG_CONFIG_ADDR_OFFSET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, offset, &config, 1) != 0))
		goto out_return;
	*PulseWidth = (config & 0x7FFFFFFF);

	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;
	if (config & 0x10)
		* Source = 1;
	else
	{
		if (config & 0x08)
			* Source = 2;
		else if (config & 0x04)
			* Source = 3;
		else
			*Source = 0;
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t __SetDeviceCmpTrigData(uint16_t DeviceNo, uint16_t CmpNo, int32_t CmpData, uint32_t Interval, uint32_t Times, uint8_t Dir)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	if (CmpNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t config;
	uint32_t config_offset, data_offset, times_offset, pitch_offset;
	if (CmpNo == 0) {
		config_offset = ENC0_CMP_TRIG_CONFIG_ADDR_OFFSET;
		data_offset = ENC0_CMP_TRIG_DATA_ADDR_OFFSET;
		times_offset = ENC0_CMP_TRIG_TIMES_ADDR_OFFSET;
		pitch_offset = ENC0_CMP_TRIG_PITCH_ADDR_OFFSET;
	} else {
		config_offset = ENC1_CMP_TRIG_CONFIG_ADDR_OFFSET;
		data_offset = ENC1_CMP_TRIG_DATA_ADDR_OFFSET;
		times_offset = ENC1_CMP_TRIG_TIMES_ADDR_OFFSET;
		pitch_offset = ENC1_CMP_TRIG_PITCH_ADDR_OFFSET;
	}

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;
	if (Dir)
		config |= (0x01 << CmpNo);
	else
		config &= ~(0x01 << CmpNo);
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, CMP_TRIG_COMM_CONFIG_ADDR_OFFSET, &config, 1) != 0))
		goto out_return;

	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, data_offset, &CmpData, 1) != 0))
		goto out_return;

	if (Times == 1) {
		config = 0;
		if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, pitch_offset, &config, 1) != 0))
			goto out_return;
	} else {
		if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, pitch_offset, &Interval, 1) != 0))
			goto out_return;
	}

	if (Times)
			config = 0x80000000 | Times;
		else
			config = 0;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, times_offset, &config, 1) != 0))
		goto out_return;

	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, config_offset, &config, 1) != 0))
		goto out_return;
	config &= 0x7FFFFFFF;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, config_offset, &config, 1) != 0))
		goto out_return;
	config |= 0x80000000;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, config_offset, &config, 1) != 0))
		goto out_return;

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceCmpTrigData(uint16_t DeviceNo, uint16_t CmpNo, int32_t CmpData)
{
	return __SetDeviceCmpTrigData(DeviceNo, CmpNo, CmpData, 0, 1, 0);
}
/*****************************************************************************/
int32_t ECAT_SetDeviceContCmpTrigData(uint16_t DeviceNo, uint16_t CmpNo, int32_t Start, uint32_t Interval, uint32_t Times, uint8_t Dir)
{
	return __SetDeviceCmpTrigData(DeviceNo, CmpNo, Start, Interval, Times, Dir);
}
/*****************************************************************************/
int32_t ECAT_SetDeviceEmg(uint16_t DeviceNo, uint8_t Source, uint8_t Enable, uint8_t Logic, uint16_t SlaveNo, uint16_t BitNo, uint8_t ServoOff)
{
	ecat_device_emg_t Ops;
	Ops.Source = Source;
	Ops.Enable = Enable;
	Ops.Logic = Logic;
	Ops.SlaveNo = SlaveNo;
	Ops.BitNo = BitNo;
	Ops.ServoOff = ServoOff;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_DEVICE_EMG,
							&Ops, sizeof(ecat_device_emg_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_GetDeviceEmg(uint16_t DeviceNo, uint8_t *Source, uint8_t *Enable, uint8_t *Logic, uint16_t *SlaveNo, uint16_t *BitNo, uint8_t *ServoOff)
{
	int32_t ret;
	ecat_device_emg_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_DEVICE_EMG,
							NULL, 0, &RetData, sizeof(ecat_device_emg_t));
	if (ret == 0) {
		*Source = RetData.Source;
		*Enable = RetData.Enable;
		*Logic = RetData.Logic;
		*SlaveNo = RetData.SlaveNo;
		*BitNo = RetData.BitNo;
		*ServoOff = RetData.ServoOff;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceEmgStatus(uint16_t DeviceNo, uint8_t *Status)
{
	int32_t ret;
	ecat_device_emg_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_DEVICE_EMG_STATUS,
							NULL, 0, &RetData, sizeof(ecat_device_emg_t));
	if (ret == 0) {
		*Status = RetData.Status;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceEmgSoftSig(uint16_t DeviceNo)
{
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_DEVICE_EMG_SIG,
							NULL, 0, NULL, 0);
}
/*****************************************************************************/
void* ThreadMpgFunc(void* arg)
{
	printf("ThreadMpgFunc Start\n");
	ecat_device_ipc_t *EcatDevice = (ecat_device_ipc_t *)arg;
	int32_t NewCnt = 0, OldCnt = 0, DiffCnt = 0;
	uint16_t OldAxisNo, AxisNo;
	uint32_t Scaling = 0, OldScaling = 0;
	uint32_t DI = 0, config;
	ecat_mc_axis_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_mc_axis_ops_t);
	Ops.AxisNo = 0;
	EcatDevice->MpgThreadRunning = 1;


	while (!EcatDevice->MpgThreadShouldStop) {
		SEMEPHORE_P_OPERATION(EcatDevice->ThreadSem)
		if (read_bar0_reg(EcatDevice->Fd, GPIO_ADDR_OFFSET, &DI, 1) != 0)
			goto sleep;

		DI = ~(DI >> 16);
		DI &= 0x1FFF;
		if (DI & ((1 << EcatDevice->MpgAxisCnt) - 1)) {
			AxisNo = 0;
			Ops.AxisNo = 0;
			Scaling = 1;
			do {
				if (DI & (1 << AxisNo)) {
					Ops.AxisNo = EcatDevice->MpgAxisNo[AxisNo];
					break;
				}
				else
					AxisNo++;

				if (AxisNo >= EcatDevice->MpgAxisCnt)
					break;

			} while (1);

			if (DI & 0x80)
				Scaling = 1;
			else if (DI & 0x100)
				Scaling = 10;
			else if (DI & 0x200)
				Scaling = 100;

			if ((Scaling != OldScaling) || (Ops.AxisNo != OldAxisNo)) {
				if (read_bar0_reg(EcatDevice->Fd, ENC0_MODE_ADDR_OFFSET, &config, 1) != 0)
					goto sleep;
				if (write_bar0_reg(EcatDevice->Fd, ENC0_MODE_ADDR_OFFSET, &config, 1) != 0)
					goto sleep;
				OldScaling = Scaling;
				OldAxisNo = Ops.AxisNo;
				NewCnt = 0;
				OldCnt = 0;
			}

			NewCnt = 0;
			if (write_bar0_reg(EcatDevice->Fd, ENC0_CNT_ADDR_OFFSET, &NewCnt, 1) != 0)
				goto sleep;
			if (read_bar0_reg(EcatDevice->Fd, ENC0_CNT_ADDR_OFFSET, &NewCnt, 1) != 0)
				goto sleep;
			DiffCnt = NewCnt - OldCnt;
			if (DiffCnt != 0) 	{
				OldCnt = NewCnt;
				Ops.I32Data = DiffCnt * Scaling;
				SetCommandData(EcatDevice, ECAT_IPC_CMD_MC_AXIS_MOVE_MPG, (uint32_t*)&Ops, OpsSize);
			}
		}
		else
			OldAxisNo = 0xFFFF;

sleep:
		SEMEPHORE_V_OPERATION(EcatDevice->ThreadSem)
		usleep(1000);
	}
	SEMEPHORE_P_OPERATION(EcatDevice->ThreadSem)
	EcatDevice->MpgThreadShouldStop = 0;
	EcatDevice->MpgThreadRunning = 0;
	SEMEPHORE_V_OPERATION(EcatDevice->ThreadSem)
	printf("ThreadMpgFunc Stop\n");
	pthread_exit(0);
}
/*****************************************************************************/
int32_t ECAT_SetDeviceMPG(uint16_t DeviceNo, uint8_t Enable, uint16_t *AxisNo, uint16_t AxisCount)
{
	uint32_t i;
	int32_t ret = 0;
	encoder_config_t config;
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)

	if (Enable) {
		if (!EcatDevice[DeviceNo].MpgThreadRunning) {
			if ((AxisCount) && (AxisCount >= MC_MPG_AXIS_NO_MAX))
				return ECAT_ERR_MC_INVALID_PARAM;

			SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
			if ((AxisCount) && (AxisNo != NULL)) {
				memcpy(EcatDevice[DeviceNo].MpgAxisNo, AxisNo, sizeof(uint16_t) * AxisCount);
				EcatDevice[DeviceNo].MpgAxisCnt = AxisCount;
			} else {
				for (i = 0; i < MC_MPG_AXIS_NO_MAX; i++)
					EcatDevice[DeviceNo].MpgAxisNo[i] = i;
				EcatDevice[DeviceNo].MpgAxisCnt = 4;
			}

			if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, ENC0_MODE_ADDR_OFFSET, &config.Cfg, 1) != 0))
				goto out_return;
			config.Mode0 = 0;
			config.EncoderMode = DEV_ENC_MODE_AB_PHASE;
			config.LPF = 0;
			if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, ENC0_MODE_ADDR_OFFSET, &config.Cfg, 1) != 0))
				goto out_return;

			EcatDevice[DeviceNo].MpgThreadShouldStop = 0;
			if ((pthread_create(&EcatDevice[DeviceNo].MpgThreadID, NULL,
					ThreadMpgFunc, (void*)&EcatDevice[DeviceNo]) != 0)) {
					ret = ECAT_ERR_IPC_CREATE_HANDLE;
			}
out_return:
			SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
			return ret;
		}
	} else {
		if ((!EcatDevice[DeviceNo].MpgThreadShouldStop) && (EcatDevice[DeviceNo].MpgThreadRunning)) {
			SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
			EcatDevice[DeviceNo].MpgThreadShouldStop = 1;
			SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
			pthread_join(EcatDevice[DeviceNo].MpgThreadID, NULL);

		}
	}
	return 0;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceMPG(uint16_t DeviceNo, uint8_t *Enable, uint16_t *AxisNo, uint16_t *AxisCount)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (EcatDevice[DeviceNo].MpgThreadRunning)
		*Enable = 0;
	else
		*Enable = 1;

	if (AxisCount)
		*AxisCount = EcatDevice[DeviceNo].MpgAxisCnt;
	if (AxisNo)
		memcpy(AxisNo, EcatDevice[DeviceNo].MpgAxisNo, sizeof(uint16_t) * EcatDevice[DeviceNo].MpgAxisCnt);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return 0;

}
/*****************************************************************************/
int32_t ECAT_GetSlaveRegister(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Addr, uint16_t DataSize, uint32_t *Val, uint8_t *state)
{
	int32_t ret;
	ecat_slave_reg_ops_t Ops;
	ecat_slave_reg_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Addr = Addr;
	Ops.DataSize = DataSize;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_REG_VALUE,
							&Ops, sizeof(ecat_slave_reg_ops_t), &RetData, sizeof(ecat_slave_reg_return_t));
	if (ret == 0)
		*Val = RetData.Val;
	else
		*state = RetData.State;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveRegister(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Addr, uint16_t DataSize, uint32_t Val, uint8_t *state)
{
	int32_t ret;
	ecat_slave_reg_ops_t Ops;
	ecat_slave_reg_return_t RetData;
	Ops.No = SlaveNo;
	Ops.Addr = Addr;
	Ops.DataSize = DataSize;
	Ops.Val = Val;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_REG_VALUE,
							&Ops, sizeof(ecat_slave_reg_ops_t), &RetData, sizeof(ecat_slave_reg_return_t));
	if (ret != 0) {
		*state = RetData.State;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetMotionRecordState(uint16_t DeviceNo, uint16_t *state, uint32_t *count)
{
	int32_t ret;
	ecat_mc_return_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_RECORD_STATE,
							NULL, 0, &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*state = RetData.U16Value;
		*count = RetData.U32Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetMotionRecord(uint16_t DeviceNo, uint16_t state)
{
	ecat_ipc_base_ops_t Ops;
	Ops.No = state;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_RECORD,
							&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_DownloadMotionLog(uint16_t DeviceNo, uint32_t *Offset, char *Data, uint32_t *DataSize, uint8_t *LastFlag)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_read_file_ops_t Ops;
	ecat_read_file_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_read_file_ops_t);
	uint32_t RetSize = sizeof(ecat_read_file_return_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (*LastFlag == 255)
	{
		Ops.GetFileSize = 1;


		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_DOWNLOAD_MOTION_LOG, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			*DataSize = RetData.Size;
			ret = 0;
			goto out_return;
		}

	}
	else
	{
		Ops.GetFileSize = 0;
		Ops.offset = *Offset;

		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_MC_DOWNLOAD_MOTION_LOG, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			memcpy(Data, RetData.Data, RetData.Size);
			*DataSize = RetData.Size;
			*Offset = RetData.Offset;
			*LastFlag = RetData.LastFlag;
			ret = 0;
			goto out_return;
		}
	}

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t timer_setup(uint16_t DeviceNo, uint32_t Interval)
{
	int32_t ret;
	ecat_u32_t Ops;
	Ops.u32data = Interval;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_TIMER,
							&Ops, sizeof(ecat_u32_t), NULL, 0);
	if ((ret == 0) && (Interval)) {
		ret = irq_setup(DeviceNo, 0x01);
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetTimer(uint16_t DeviceNo, uint32_t Interval)
{
	return timer_setup(DeviceNo, Interval);
}
/*****************************************************************************/
int32_t ECAT_SetTimerStop(uint16_t DeviceNo)
{
	return timer_setup(DeviceNo, 0);
}
/*****************************************************************************/
int32_t ECAT_WaitforTimer(uint16_t DeviceNo)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int ret = 0;

	pthread_mutex_lock(&EcatDevice[DeviceNo].cond_timer_mutex);

	if (pthread_cond_wait(&EcatDevice[DeviceNo].cond_timer, &EcatDevice[DeviceNo].cond_timer_mutex) != 0)
		ret = ECAT_ERR_DRV_IOCTL;


	pthread_mutex_unlock(&EcatDevice[DeviceNo].cond_timer_mutex);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetStewartPlatform_M1(uint16_t DeviceNo, double radiusB, double angleB, double radiusP, double angleP, double RodLength, double Max_RodLength)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int32_t ret;
	ecat_stewart_t Ops;
	ecat_slave_reg_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_stewart_t);
	uint32_t RetSize = sizeof(ecat_slave_reg_return_t);

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	stewartPlatform_t *Sp = &EcatDevice[DeviceNo].StewartPlatform;
	int i;
	Sp->rb = radiusB;
	Sp->rp = radiusP;
	Sp->ab = angleB / 180 * M_PI;
	Sp->ap = angleP / 180 * M_PI;
	Sp->ab_degree = angleB;
	Sp->ap_degree = angleP;

	for (i = 0; i<6; i++)
	{
		Sp->L0[i] = RodLength;
	}
	Sp->Z0 = sqrt(Sp->L0[0] * Sp->L0[0] - (Sp->rb - Sp->rp)*(Sp->rb - Sp->rp));
	/******************************/

	i = 1;
	Sp->angP[i] = i*M_PI / 3 - 0.5*Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;
	i = 2;
	Sp->angP[i] = Sp->angP[i - 1] + Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;
	i = 3;
	Sp->angP[i] = i*M_PI / 3 - 0.5*Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;
	i = 4;
	Sp->angP[i] = Sp->angP[i - 1] + Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;
	i = 5;
	Sp->angP[i] = i*M_PI / 3 - 0.5*Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;
	i = 6;
	Sp->angP[i] = Sp->angP[i - 1] + Sp->ap;
	Sp->Px[i] = Sp->rp * cos(Sp->angP[i]);
	Sp->Py[i] = Sp->rp * sin(Sp->angP[i]);
	Sp->Pz[i] = 0;

	/******************************/
	i = 1;
	Sp->angB[i] = i*M_PI / 3 - 0.5*Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;
	i = 2;
	Sp->angB[i] = Sp->angB[i - 1] + Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;
	i = 3;
	Sp->angB[i] = i*M_PI / 3 - 0.5*Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;
	i = 4;
	Sp->angB[i] = Sp->angB[i - 1] + Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;
	i = 5;
	Sp->angB[i] = i*M_PI / 3 - 0.5*Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;
	i = 6;
	Sp->angB[i] = Sp->angB[i - 1] + Sp->ab;
	Sp->Bx[i] = Sp->rb * cos(Sp->angB[i]);
	Sp->By[i] = Sp->rb * sin(Sp->angB[i]);
	Sp->Bz[i] = 0;

	for (i = 0; i<6; i++)
	{
		Sp->L0[i] = RodLength;
		Sp->Lmin[i] = RodLength;
		Sp->Lmax[i] = Max_RodLength;
	}
	for (i = 0; i<6; i++)
	{
		Ops.Bx[i] = Sp->Bx[i + 1];
		Ops.By[i] = Sp->By[i + 1];
		Ops.Px[i] = Sp->Px[i + 1];
		Ops.Py[i] = Sp->Py[i + 1];
	}
	Ops.Z0 = Sp->Z0;
	memcpy(Ops.Max_RodLength, Sp->Lmax, sizeof(Sp->Lmax));
	memcpy(Ops.Min_RodLength, Sp->Lmin, sizeof(Sp->Lmin));
	if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_SET_STEWART_PLATFORM, (uint32_t*)&Ops, OpsSize)) < 0)
		goto out_return;

	ret = GetReturnData(&EcatDevice[DeviceNo], NULL, 0);

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (ret == 0)
		Sp->setup = 1;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualPosVel(uint16_t DeviceNo, uint16_t AxisNo, float *Pos, float *Vel)
{
	int32_t ret;
	int i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_float_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACT_POS_VEL,
							&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_mc_return_float_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++) {
				*(Pos + i) = RetData.FloatValue_multi_Value1[i];
				*(Vel + i) = RetData.FloatValue_multi_Value2[i];
			}
		} else {
			*Pos = RetData.FloatValue_multi_Value1[0];
			*Vel = RetData.FloatValue_multi_Value2[0];
		}
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidSetSetPointValue(uint16_t DeviceNo, uint32_t PidNo, double SetPointValue)
{
	Pid_ecat_double_t Ops;
	Ops.PidNo = PidNo;
	Ops.doubledata = SetPointValue;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_SET_POINT_VALUE,
							&Ops, sizeof(Pid_ecat_double_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidGetControlOutputValue(uint16_t DeviceNo, uint32_t PidNo, double *Output)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_PARAMETER,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_t));
	if (ret == 0) {
		*Output = RetData.Feedback;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidSetParameter(uint16_t DeviceNo, uint32_t PidNo, double kp, double ki, double kd)
{
	ecat_pid_t Ops;
	Ops.PidNo = PidNo;
	Ops.kp = kp;
	Ops.ki = ki;
	Ops.kd = kd;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_PARAMETER,
							&Ops, sizeof(ecat_pid_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidSetStatus(uint16_t DeviceNo, uint32_t PidNo, uint8_t status)
{
	ecat_pid_t Ops;
	Ops.PidNo = PidNo;
	Ops.status = status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_STATUS,
							&Ops, sizeof(ecat_pid_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidSetSimulateMode(uint16_t DeviceNo, uint32_t PidNo, uint8_t status)
{
	ecat_pid_t Ops;
	Ops.PidNo = PidNo;
	Ops.status = status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_SIMULATE,
							&Ops, sizeof(ecat_pid_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidSetSampleTime(uint16_t DeviceNo, uint32_t PidNo, uint32_t Interval)
{
	ecat_pid_controller_t Ops;
	Ops.PidNo = PidNo;
	Ops.Interval = Interval;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_CONTROLLER,
							&Ops, sizeof(ecat_pid_controller_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidSetProcessVariableModule(uint16_t DeviceNo, uint32_t PidNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t Bitlength, double ScaleGain, double ScaleOffset)
{
	ecat_pid_controller_t Ops;
	Ops.PidNo = PidNo;
	Ops.SlaveNo_In = SlaveNo;
	Ops.OffsetByte_In = OffsetByte;
	Ops.Bitlength_In = Bitlength;
	Ops.Scalegain_In = ScaleGain;
	Ops.Scaleoffset_In = ScaleOffset;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_PROCESS_VARIABLE_MODULE,
							&Ops, sizeof(ecat_pid_controller_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidSetControlOutputModule(uint16_t DeviceNo, uint32_t PidNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t Bitlength, double ScaleGain, double ScaleOffset, double Output_Max_Value, double Output_Min_Value)
{
	ecat_pid_controller_t Ops;
	Ops.PidNo = PidNo;
	Ops.SlaveNo_Out = SlaveNo;
	Ops.OffsetByte_Out = OffsetByte;
	Ops.Bitlength_Out = Bitlength;
	Ops.Scalegain_Out = ScaleGain;
	Ops.Scaleoffset_Out = ScaleOffset;
	Ops.Output_max = Output_Max_Value;
	Ops.Output_min = Output_Min_Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PID_CONTROL_OUTPUT_MODULE,
							&Ops, sizeof(ecat_pid_controller_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_PidGetParameter(uint16_t DeviceNo, uint32_t PidNo, double *kp, double *ki, double *kd)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_PARAMETER,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_t));
	if (ret == 0) {
		*kp = RetData.kp;
		*ki = RetData.ki;
		*kd = RetData.kd;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetStatus(uint16_t DeviceNo, uint32_t PidNo, uint8_t *status)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_STATUS,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_t));
	if (ret == 0) {
		*status = RetData.status;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetSimulateMode(uint16_t DeviceNo, uint32_t PidNo, uint8_t *status)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_SIMULATE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_t));
	if (ret == 0) {
		*status = RetData.status;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetSampleTime(uint16_t DeviceNo, uint32_t PidNo, uint32_t *Interval)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_controller_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_CONTROLLER,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_controller_t));
	if (ret == 0) {
		*Interval = RetData.Interval;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetProcessVariableModule(uint16_t DeviceNo, uint32_t PidNo, uint16_t *SlaveNo, uint16_t *OffsetByte, uint16_t *Bitlength, double *ScaleGain, double *ScaleOffset)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_controller_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_PROCESS_VARIABLE_MODULE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_controller_t));
	if (ret == 0) {
		*SlaveNo = RetData.SlaveNo_In;
		*OffsetByte = RetData.OffsetByte_In;
		*Bitlength = RetData.Bitlength_In;
		*ScaleGain = RetData.Scalegain_In;
		*ScaleOffset = RetData.Scaleoffset_In;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetControlOutputModule(uint16_t DeviceNo, uint32_t PidNo, uint16_t *SlaveNo, uint16_t *OffsetByte, uint16_t *Bitlength, double *ScaleGain, double *ScaleOffset, double *Output_Max_Value, double *Output_Min_Value)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_controller_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_CONTROL_OUTPUT_MODULE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_controller_t));
	if (ret == 0) {
		*SlaveNo = RetData.SlaveNo_Out;
		*OffsetByte = RetData.OffsetByte_Out;
		*Bitlength = RetData.Bitlength_Out;
		*ScaleGain = RetData.Scalegain_Out;
		*ScaleOffset = RetData.Scaleoffset_Out;
		*Output_Max_Value = RetData.Output_max;
		*Output_Min_Value = RetData.Output_min;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetProcessVariable(uint16_t DeviceNo, uint32_t PidNo, double *ProcessVariable)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	Pid_ecat_double_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_CONTROL_OUTPUT_MODULE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(Pid_ecat_double_t));
	if (ret == 0) {
		*ProcessVariable = RetData.doubledata;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetSimulateFeedback(uint16_t DeviceNo, uint32_t PidNo, double *Feedback)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	ecat_pid_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_CONTROL_OUTPUT_MODULE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(ecat_pid_t));
	if (ret == 0) {
		*Feedback = RetData.Feedback;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGetSetPointValue(uint16_t DeviceNo, uint32_t PidNo, double *SetPointValue)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	Pid_ecat_double_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_SET_POINT_VALUE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(Pid_ecat_double_t));
	if (ret == 0) {
		*SetPointValue = RetData.doubledata;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_PidGet_Sp_Err_Op_Pv(uint16_t DeviceNo, uint32_t PidNo, double *SetPointValue, double *Error, double *OutputValue, double *ProcessVariable)
{
	int32_t ret;
	Pid_ecat_32_t Ops;
	Pid_ecat_in_out_t RetData;
	Ops.PidNo = PidNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PID_IN_OUT_VALUE,
							&Ops, sizeof(Pid_ecat_32_t), &RetData, sizeof(Pid_ecat_in_out_t));
	if (ret == 0) {
		*SetPointValue = RetData.SetPointValue;
		*Error = RetData.Error;
		*OutputValue = RetData.OutPutValue;
		*ProcessVariable = RetData.ProcessVariable;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisVelocityFeedForwardGain(uint16_t DeviceNo, uint16_t AxisNo, double Gain)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Gain;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_FEEDFORWARD_GAIN,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisVelocityFeedForwardGain(uint16_t DeviceNo, uint16_t AxisNo, double *Gain)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_FEEDFORWARD_GAIN,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Gain = RetData.DoubleValue[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McStewartPlatformMoveAbs_PT(uint16_t DeviceNo, uint16_t GroupNo, double Pose[], double *Pos, double Time)
{
	int32_t ret;
	ecat_stewart_Pose_t Ops;
	ecat_stewart_return_t RetData;
	Ops.GroupNo = GroupNo;
	Ops.time = Time;
	memcpy(Ops.Pose, Pose, sizeof(Ops.Pose));
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_STEWART_PLATFORM_MOVE_ABS_PT,
							&Ops, sizeof(ecat_stewart_Pose_t), &RetData, sizeof(ecat_stewart_return_t));
	if (ret == 0) {
		memcpy(Pos, RetData.Pos, sizeof(RetData.Pos));
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetStewartPlatform_M2(uint16_t DeviceNo, double Bx[], double By[], double Px[], double Py[], double Z0, double RodLength[], double Max_RodLength[])
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	ecat_stewart_t Ops;
	ecat_slave_reg_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_stewart_t);
	uint32_t RetSize = sizeof(ecat_slave_reg_return_t);
	int i, j, error = 0;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	stewartPlatform_t *Sp = &EcatDevice[DeviceNo].StewartPlatform;

	int32_t ret = 0;
	Sp->Z0 = Z0;
	/******************************/
	for (i = 1; i < 7; i++)
	{
		Sp->Px[i] = Px[i - 1];
		Sp->Py[i] = Py[i - 1];
		Sp->Pz[i] = Z0;

		Sp->Bx[i] = Bx[i - 1];
		Sp->By[i] = By[i - 1];
		Sp->Bz[i] = 0;
	}


	double L[6], dL[6];
	double ca, cb, cr, sa, sb, sr;
	double r11, r12, r13, r21, r22, r23, r31, r32, r33;
	double Xbar[6], Ybar[6], Zbar[6];
	double U[6], V[6], W[6];
	double x, y, z, a, b, r;

	x = 0;
	y = 0;
	z = Sp->Z0;
	a = 0;
	b = 0;
	r = 0;


	ca = cos(a);
	cb = cos(b);
	cr = cos(r);
	sa = sin(a);
	sb = sin(b);
	sr = sin(r);

	r11 = cb*cr;
	r12 = sa*sb*cr - ca*sr;
	r13 = ca*sb*cr + sa*sr;
	r21 = cb*sr;
	r22 = sa*sb*sr + ca*cr;
	r23 = ca*sb*sr - sa*cr;
	r31 = -sb;
	r32 = sa*cb;
	r33 = ca*cb;

	for (i = 0; i<6; i++)
	{
		Xbar[i] = x - Sp->Bx[i + 1];
		Ybar[i] = y - Sp->By[i + 1];
		Zbar[i] = z;
		U[i] = r11*Sp->Px[i + 1] + r12*Sp->Py[i + 1];
		V[i] = r21*Sp->Px[i + 1] + r22*Sp->Py[i + 1];
		W[i] = r31*Sp->Px[i + 1] + r32*Sp->Py[i + 1];
	}


	for (i = 0; i<6; i++)
	{
		L[i] = sqrt((Xbar[i] + U[i]) * (Xbar[i] + U[i]) + (Ybar[i] + V[i]) * (Ybar[i] + V[i]) + (Zbar[i] + W[i]) * (Zbar[i] + W[i]));
		Sp->L0[i] = L[i];
		Sp->Lmin[i] = L[i];
		Sp->Lmax[i] = Max_RodLength[i];

	}

	for (i = 0; i<6; i++)
	{
		Ops.Bx[i] = Sp->Bx[i + 1];
		Ops.By[i] = Sp->By[i + 1];
		Ops.Px[i] = Sp->Px[i + 1];
		Ops.Py[i] = Sp->Py[i + 1];
	}
	Ops.Z0 = Sp->Z0;
	memcpy(Ops.Max_RodLength, Sp->Lmax, sizeof(Sp->Lmax));
	memcpy(Ops.Min_RodLength, Sp->Lmin, sizeof(Sp->Lmin));
	if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_SET_STEWART_PLATFORM, (uint32_t*)&Ops, OpsSize)) < 0)
		goto out_return;

	ret = GetReturnData(&EcatDevice[DeviceNo], NULL, 0);

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (ret == 0)
		Sp->setup = 1;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetStewartPlatform_M1(uint16_t DeviceNo, double *radiusB, double *angleB, double *radiusP, double *angleP, double *RodLength, double *Max_RodLength)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	stewartPlatform_t *Sp = &EcatDevice[DeviceNo].StewartPlatform;

	if (Sp->setup != 1)
		return ECAT_ERR_MC_NOT_SETUP;
	*radiusB = Sp->rb;
	*radiusP = Sp->rp;
	*angleB = Sp->ab_degree;
	*angleP = Sp->ap_degree;
	*Max_RodLength = Sp->Lmax[0];
	*RodLength = Sp->L0[0];

	return 0;
}
/*****************************************************************************/
int32_t ECAT_McGetStewartPlatform_M2(uint16_t DeviceNo, double *Bx, double *By, double *Px, double *Py, double *Z0, double *RodLength, double *Max_RodLength)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	stewartPlatform_t *Sp = &EcatDevice[DeviceNo].StewartPlatform;

	if (Sp->setup != 1)
		return ECAT_ERR_MC_NOT_SETUP;
	int i;
	for (i = 0; i<6; i++)
	{
		Bx[i] = Sp->Bx[i + 1];
		By[i] = Sp->By[i + 1];
		Px[i] = Sp->Px[i + 1];
		Py[i] = Sp->Py[i + 1];
		Max_RodLength[i] = Sp->Lmax[i];
		RodLength[i] = Sp->L0[i];
	}
	*Z0 = Sp->Z0;
	return 0;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupVelLimitStatus(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Status)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_VEL_LIMIT_STATUS,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupVelLimitStatus(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *Status)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_VEL_LIMIT_STATUS,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Status = RetData.U16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupVelLimitValue(uint16_t DeviceNo, uint16_t GroupNo, double Value)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.DoubleData[0] = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_VEL_LIMIT_VALUE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupVelLimitValue(uint16_t DeviceNo, uint16_t GroupNo, double *Value)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_VEL_LIMIT_VALUE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Value = RetData.DoubleValue[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetMotionRecordParam(uint16_t DeviceNo, uint16_t Value1, uint16_t Value2)
{
	ecat_mc_axis_ops_t Ops;
	Ops.U16Data[0] = Value1;
	Ops.U16Data[1] = Value2;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_RECORD_PARAM,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetMotionRecordParam(uint16_t DeviceNo, uint16_t *Value1, uint16_t *Value2)
{
	int32_t ret;
	ecat_mc_axis_ops_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_RECORD_PARAM,
							NULL, 0, &RetData, sizeof(ecat_mc_axis_ops_t));
	if (ret == 0) {
		*Value1 = RetData.U16Data[0];
		*Value2 = RetData.U16Data[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetMotionRecordValue(uint16_t DeviceNo, uint32_t CountNo, uint16_t AxisNo, float *Value1, float *Value2)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	ecat_mc_return_float_multi_t RetData;
	uint32_t i;
	Ops.AxisNo = AxisNo;
	Ops.U32Data = CountNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_RECORD_VALUE,
							&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_mc_return_float_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++) {
				*(Value1 + i) = RetData.FloatValue_multi_Value1[i];
				*(Value2 + i) = RetData.FloatValue_multi_Value2[i];
			}
		} else 	{
			*Value1 = RetData.FloatValue_multi_Value1[0];
			*Value2 = RetData.FloatValue_multi_Value2[0];
		}
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McClearMotionRecord(uint16_t DeviceNo)
{

	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_CLEAR_RECORD,
							NULL, 0, NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetAxisPosSoftwareLimit(uint16_t DeviceNo, uint16_t AxisNo, double Maximum, double Minimum)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Minimum;
	Ops.DoubleData[1] = Maximum;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_SOFTWARE_LIMIT,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisPosSoftwareLimit(uint16_t DeviceNo, uint16_t AxisNo, double *Maximum, double *Minimum)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_SOFTWARE_LIMIT,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Minimum = RetData.DoubleValue[0];
		*Maximum = RetData.DoubleValue[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetProcessTime(uint16_t DeviceNo, double *Time)
{
	int32_t ret;
	ecat_double_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PROCESS_TIME,
							NULL, 0, &RetData, sizeof(ecat_double_t));
	if (ret == 0) {
		*Time = RetData.doubledata;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupHold(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Status)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_HOLD,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetAxisPosSoftwareLimitStatus(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Status, uint16_t ErrorStop)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Status;
	Ops.U16Data[1] = ErrorStop;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_POS_SW_STATUS,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisPosSoftwareLimitStatus(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Status, uint16_t *ErrorStop)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_axis_ops_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_POS_SW_STATUS,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_axis_ops_t));
	if (ret == 0) {
		*Status = RetData.U16Data[0];
		*ErrorStop = RetData.U16Data[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeState(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *State)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_STATE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*State = RetData.U16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryIn(uint16_t DeviceNo, uint16_t MasterNo, uint16_t SlaveNo, int32_t Direction)
{
	ecat_mc_axis_ops_t Ops;
	Ops.MasterNo = MasterNo;
	Ops.AxisNo = SlaveNo;
	Ops.I32Data = Direction;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRY_IN,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetGroupCmdModeEx(uint16_t DeviceNo, uint16_t GroupNo, uint16_t CmdMode)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = CmdMode;
	Ops.U16Data[1] = 1; //put this action(mode changing) in the buffer
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_CMD_MODE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryMaxPosDiff(uint16_t DeviceNo, uint16_t SlaveNo, double Value)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = SlaveNo;
	Ops.DoubleData[0] = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRY_MAX_POS_DIFF,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryMaxPosDiffStatus(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Status)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = SlaveNo;
	Ops.U16Data[0] = Status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRY_MAX_POS_DIFF_STATUS,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvEnableEvent(uint16_t DeviceNo, uint16_t EventID)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_ENABLE_EVENT,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvDisableEvent(uint16_t DeviceNo, uint16_t EventID)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_DISABLE_EVENT,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_WaitforEvent(uint16_t DeviceNo, uint32_t TimeOut, uint32_t *TriggeredEvent)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	int ret = 0;
	uint32_t event_state;
	struct timespec ts_now, ts_timeout;
	int64_t timeout_ns;

	pthread_mutex_lock(&EcatDevice[DeviceNo].cond_event_mutex);

	if (TimeOut == 0xFFFFFFFF) {
		if (pthread_cond_wait(&EcatDevice[DeviceNo].cond_event, &EcatDevice[DeviceNo].cond_event_mutex) != 0)
			ret = ECAT_ERR_DRV_IOCTL;
	} else {
		clock_gettime(CLOCK_REALTIME, &ts_now);
		timeout_ns = ((int64_t)TimeOut * 1000000);
		timespec_add_nsec(&ts_timeout, &ts_now, timeout_ns);
		if ((ret = pthread_cond_timedwait(&EcatDevice[DeviceNo].cond_event,
				&EcatDevice[DeviceNo].cond_event_mutex, &ts_timeout)) != 0) {
			if (ret == ETIMEDOUT)
				ret = ECAT_ERR_IPC_TIME_OUT;
			else
				ret = ECAT_ERR_DRV_IOCTL;
		}
	}

	pthread_mutex_unlock(&EcatDevice[DeviceNo].cond_event_mutex);
	if(ret == 0) {
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		read_dpram(EcatDevice[DeviceNo].Fd, 0x7FD, &event_state, 1);
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		*TriggeredEvent = event_state;
		return 0;
	}else
		return ret;
}
/*****************************************************************************/
int32_t ECAT_EvSetComparePositionParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t AxisNo, uint16_t Operator, double ComparePosition)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Operator;
	Ops.DoubleData[0] = ComparePosition;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_POSITION_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareDIBitParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t SlaveNo, uint16_t BitNo, uint32_t CompareValue)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.SlaveNo = SlaveNo;
	Ops.U16Data[0] = BitNo;
	Ops.U32Data[0] = CompareValue;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_DIBIT_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetMotionCompleteParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t AxisNo)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_MOTION_COMPLETE_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveAO(uint16_t DeviceNo, uint16_t GroupNo, uint16_t SlaveNo, uint32_t RunMode, uint16_t ChannelNo, uint16_t RawData, double VoltData)
{
	ecat_mc_group_move_ao_ops_t Ops;
	Ops.RunMode = RunMode;
	Ops.GroupNo = GroupNo;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	Ops.RawData = RawData;
	Ops.VoltData = VoltData;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_AO,
							&Ops, sizeof(ecat_mc_group_move_ao_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetGroupAccTimeEx(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Time_ms)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Time_ms;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_ACC_TIME_EX,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetGroupBlendingPercent(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Value)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_BLENDING_PERCENT,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetProfileInterval(uint16_t DeviceNo, uint16_t ProfileNo, uint16_t Value)
{
	ecat_mc_axis_ops_t Ops;
	Ops.U16Data[0] = ProfileNo;
	Ops.U16Data[1] = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_PROFILE_INTERVAl,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveSuperimposed(uint16_t DeviceNo, uint16_t AxisNo, double Pos, double Vel)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	Ops.Vel = Vel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_SUPERIMPOSED,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisHaltSuperimposed(uint16_t DeviceNo, uint16_t AxisNo)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_HALT_SUPERIMPOSED,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetMotionRecordValueEx(uint16_t DeviceNo, uint32_t CountNo, uint16_t Count, uint16_t AxisNo, float *Value1, float *Value2, uint16_t *ActualCount)
{
	int32_t ret;
	uint16_t i;
	ecat_mc_axis_ops_t Ops;
	ecat_mc_return_float_multi_t RetData;
	Ops.AxisNo = AxisNo;
	Ops.U32Data = CountNo;
	Ops.U16Data[0] = Count;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_RECORD_VALUE_EX,
							&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_mc_return_float_multi_t));
	if (ret == 0) {
		*ActualCount = RetData.ActualCount;
		for (i = 0; i < *ActualCount; i++) {
			*(Value1 + i) = RetData.FloatValue_multi_Value1[i];
			*(Value2 + i) = RetData.FloatValue_multi_Value2[i];
		}
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetHeartBeat(uint16_t DeviceNo, uint32_t Value)
{
	ecat_u32_t Ops;
	Ops.u32data = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_HEARTBEAT,
							&Ops, sizeof(ecat_u32_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SetHeartBeatStatus(uint16_t DeviceNo, uint32_t Value)
{
	ecat_u32_t Ops;
	Ops.u32data = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_HEARTBEAT_STATUS,
							&Ops, sizeof(ecat_u32_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetGroupBlendingPercentEx(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Value)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Value;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_BLENDING_PERCENT_EX,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareDIParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t SlaveNo, uint16_t OffsetByte, uint32_t CompareValue, uint32_t Mask)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.SlaveNo = SlaveNo;
	Ops.U16Data[0] = OffsetByte;
	Ops.U32Data[0] = CompareValue;
	Ops.U32Data[1] = Mask;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_DI_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetGroupPause(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Status)
{
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Status;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_PAUSE,
							&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareAxisStateParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t AxisNo, uint32_t CompareState)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = AxisNo;
	Ops.U32Data[0] = CompareState;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_AXIS_STATE_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveAiProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t Range)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_SET_AI_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	Ops.Range = Range;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAiProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t *Range)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AI_PROPERTY;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Range = Ops.Range;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAiRawData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, int16_t *Data)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AI_RAW_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Data = Ops.RawData;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAiVoltData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, double *Data)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AI_VOLT_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Data = Ops.VoltData;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAimAData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, double *Data)
{
	int32_t ret;
	ecat_slave_aio_data_t Ops;
	Ops.RunMode = SLAVE_AIO_RM_GET_AI_mA_DATA;
	Ops.SlaveNo = SlaveNo;
	Ops.ChannelNo = ChannelNo;
	ret = CommonSlaveAIO(DeviceNo, &Ops);
	if (ret == 0)
		*Data = Ops.mAData;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisHomeEx(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Settings)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
    Ops.U16Data[0] = Settings;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_HOME_EX,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareCmdPositionParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t AxisNo, uint16_t Operator, double ComparePosition)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Operator;
	Ops.DoubleData[0] = ComparePosition;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_COMMAND_POSITION_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareAxisVelStateParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t AxisNo, uint32_t CompareState)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = AxisNo;
    Ops.U32Data[0] = CompareState;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_AXIS_VEL_STATE_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveVelEx(uint16_t DeviceNo, uint16_t GroupNo, double Vel[], uint32_t* CmdIndex)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
	uint32_t cmd_idx;

	Ops.GroupNo = GroupNo;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.DoubleData, Vel, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_VEL,
		&Ops, sizeof(ecat_mc_group_ops_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxisNo(uint16_t DeviceNo, uint16_t GroupNo, uint16_t AxisNo[], uint16_t* AxisCnt)
{
	int32_t ret, i;
	ecat_mc_group_axis_no_ops_t Ops;

	Ops.GroupNo = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXIS_NO,
		&Ops, sizeof(ecat_mc_group_axis_no_ops_t), &Ops, sizeof(ecat_mc_group_axis_no_ops_t));
	if (ret == 0) {
		if (AxisCnt)
			* AxisCnt = Ops.AxisCnt;
		if (AxisNo) {
			for (i = 0; i < Ops.AxisCnt; i++)
				AxisNo[i] = Ops.AxisNo[i];
		}
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupLastError(uint16_t DeviceNo, uint16_t GroupNo, int32_t* Error)
{
	int32_t ret;
	ecat_mc_return_t Ops;

	Ops.U16Value = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_LAST_ERROR,
		&Ops, sizeof(ecat_mc_return_t), &Ops, sizeof(ecat_mc_return_t));

	if (ret == 0)
		* Error = Ops.I32Value;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupErrorReset(uint16_t DeviceNo, uint16_t GroupNo)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_ERROR_RESET,
		&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupMotionState(uint16_t DeviceNo, uint16_t GroupNo, uint32_t* State)
{
	int32_t ret;
	ecat_mc_return_t Ops;
	Ops.U16Value = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_MOTION_STATE,
		&Ops, sizeof(ecat_mc_return_t), &Ops, sizeof(ecat_mc_return_t));
	if (ret == 0)
		* State = Ops.U32Value;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxesActualPos(uint16_t DeviceNo, uint16_t GroupNo, double* Pos, uint16_t Arr)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
	Ops.GroupNo = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXES_ACT_POS,
		&Ops, sizeof(ecat_mc_group_ops_t), &Ops, sizeof(ecat_mc_group_ops_t));
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (Arr == EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
			memcpy(Pos, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		else if (Arr > EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
		{
			uint16_t AxesCnt = EcatDevice[DeviceNo].GrpAxisCnt[GroupNo];

			memset(&Pos[AxesCnt], 0, sizeof(double) * (Arr - AxesCnt));
			memcpy(Pos, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		}
		else
		{
			memcpy(Pos, Ops.DoubleData, sizeof(double) * Arr);
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxesActualVel(uint16_t DeviceNo, uint16_t GroupNo, double* Vel, uint16_t Arr)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXES_ACT_VEL,
		&Ops, sizeof(ecat_mc_group_ops_t), &Ops, sizeof(ecat_mc_group_ops_t));

	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (Arr == EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
			memcpy(Vel, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		else if (Arr > EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
		{
			uint16_t AxesCnt = EcatDevice[DeviceNo].GrpAxisCnt[GroupNo];

			memset(&Vel[AxesCnt], 0, sizeof(double) * (Arr - AxesCnt));
			memcpy(Vel, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		}
		else
		{
			memcpy(Vel, Ops.DoubleData, sizeof(double) * Arr);
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
		return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_stick_control_enable(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Enable)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Enable;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_STICK_CONTROL_ENABLE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_breakout_force(uint16_t DeviceNo, uint16_t AxisNo, float ForcePos, float ForceNeg, float Deg)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = ForcePos;
	Ops.floatData[1] = ForceNeg;
	Ops.floatData[2] = Deg;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BREAKOUT_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_operation_range(uint16_t DeviceNo, uint16_t AxisNo, float PostiveDeg, float NegativeDeg)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = PostiveDeg;
	Ops.floatData[1] = NegativeDeg;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_OPERATION_RANGE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_coulomb_force(uint16_t DeviceNo, uint16_t AxisNo, float CoulombRefPos, float CoulombRefNeg, float VelocityZone)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = CoulombRefPos;
	Ops.floatData[1] = CoulombRefNeg;
	Ops.floatData[2] = VelocityZone;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_COULOMB_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_control_mode(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Mode)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Mode;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_CONTROL_MODE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_end_stop(uint16_t DeviceNo, uint16_t AxisNo, float EndStopK, float EndStopZeta)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = EndStopK;
	Ops.floatData[1] = EndStopZeta;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_END_STOP,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_gradient_multiplier(uint16_t DeviceNo, uint16_t AxisNo, float Multiplier)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Multiplier;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_MULTIPLIER,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_reference_model_MBK(uint16_t DeviceNo, uint16_t AxisNo, float Mass, float Damping, float Stiff)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass;
	Ops.floatData[1] = Damping;
	Ops.floatData[2] = Stiff;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_REFERENCE_MODEL_MBK,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_reference_model_MZetaWn(uint16_t DeviceNo, uint16_t AxisNo, float Mass, float Zeta, float Wn)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass;
	Ops.floatData[1] = Zeta;
	Ops.floatData[2] = Wn;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_REFERENCE_MODEL_MZETAWN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_gradient_zone_ratio(uint16_t DeviceNo, uint16_t AxisNo, uint16_t r1, uint16_t r2, uint16_t r3, uint16_t n1, uint16_t n2, uint16_t n3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = r1;
	Ops.U16Data[1] = r2;
	Ops.U16Data[2] = r3;
	Ops.U16Data[3] = n1;
	Ops.U16Data[4] = n2;
	Ops.U16Data[5] = n3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_ZONE_RATIO,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_gradient_positive(uint16_t DeviceNo, uint16_t AxisNo, float g0, float g1, float g2, float g3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = g0;
	Ops.floatData[1] = g1;
	Ops.floatData[2] = g2;
	Ops.floatData[3] = g3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_POSITIVE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_gradient_negative(uint16_t DeviceNo, uint16_t AxisNo, float g0, float g1, float g2, float g3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = g0;
	Ops.floatData[1] = g1;
	Ops.floatData[2] = g2;
	Ops.floatData[3] = g3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_NEGATIVE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_limit(uint16_t DeviceNo, uint16_t AxisNo, float Limit)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Limit;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_LIMIT,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_dead_zone(uint16_t DeviceNo, uint16_t AxisNo, float Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Force;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_DEAD_ZONE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_force_error(uint16_t DeviceNo, uint16_t AxisNo, float* Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FORCE_ERROR,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(Force + i) = RetData.FloatValue_Multi[i];
			}
		else
			*Force = RetData.FloatValue_Multi[0];
	}

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_max_velocity(uint16_t DeviceNo, uint16_t AxisNo, float Velocity)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Velocity;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_MAX_VELOCITY,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_max_acceleration(uint16_t DeviceNo, uint16_t AxisNo, float Acc)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Acc;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_MAX_ACCELERATION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_sensor_parameters(uint16_t DeviceNo, uint16_t AxisNo, uint16_t SlaveNo)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = SlaveNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_SENSOR_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_sensor_max_voltage(uint16_t DeviceNo, uint16_t AxisNo, float Voltage)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Voltage;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_SENSOR_MAX_VOLTAGE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_sensor_max_torque(uint16_t DeviceNo, uint16_t AxisNo, float Torque)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Torque;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_SENSOR_MAX_TORQUE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_operation_range_margin(uint16_t DeviceNo, uint16_t AxisNo, float Margin)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Margin;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_OPERATION_RANGE_MARGIN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_trim_neutral_position(uint16_t DeviceNo, uint16_t AxisNo)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_NEUTRAL_POSITION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_control_mode_default(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Mode)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Mode;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_CONTROL_MODE_DEFAULT,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_control_mode0_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Mass0, float Zeta0, float Wn0, float MaxAcc0, float MaxVel0, float Coulomb0)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass0;
	Ops.floatData[1] = Zeta0;
	Ops.floatData[2] = Wn0;
	Ops.floatData[3] = MaxAcc0;
	Ops.floatData[4] = MaxVel0;
	Ops.floatData[5] = Coulomb0;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_CONTROL_MODE0_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_trim_enable(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Enable)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Enable;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_ENABLE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_trim_offset(uint16_t DeviceNo, uint16_t AxisNo, float Offset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Offset;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_OFFSET,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StGet_axis_trim_offset(uint16_t DeviceNo, uint16_t AxisNo, float* Offset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_TRIM_OFFSET,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));

	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(Offset + i) = RetData.FloatValue_Multi[i];
			}
		else
			*Offset = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_StGet_trim_state(uint16_t DeviceNo, uint16_t* State)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_TRIM_STATE,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
		* State = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_trim_release(uint16_t DeviceNo, uint16_t AxisNo, uint16_t on_off)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = on_off;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_RELEASE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_stick_linking(uint16_t DeviceNo, uint16_t Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.U16Data[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_STICK_LINKING,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_model_V(uint16_t DeviceNo, uint16_t AxisNo, float* V)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_MODEL_V,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(V + i) = RetData.FloatValue_Multi[i];
			}
		else
			*V = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_model_A(uint16_t DeviceNo, uint16_t AxisNo, float* A)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_MODEL_A,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(A + i) = RetData.FloatValue_Multi[i];
			}
		else
			*A = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_average_time(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Time)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Time;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_AVERAGE_TIME,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_filter_time_constant(uint16_t DeviceNo, uint16_t AxisNo, float Time_const)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Time_const;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_FILTER_TIME_CONSTANT,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_trip_state(uint16_t DeviceNo, uint16_t AxisNo, uint16_t* State)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_TRIP_STATE,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(State + i) = RetData.U16Value_Multi[i];
			}
		else
			*State = RetData.U16Value_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ad_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AD_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_position_error_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_POSITION_ERROR_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_acceleration_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_ACCELERATION_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_velocity_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_VELOCITY_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_positive_travel_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_POSITIVE_TRAVEL_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_negative_travel_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_NEGATIVE_TRAVEL_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_power_trip_level(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_POWER_TRIP_LEVEL,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_force_gain(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_FORCE_GAIN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_position_gain(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_POSITION_GAIN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_shaker_activate_mode(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Mode)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Mode;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_SHAKER_ACTIVATE_MODE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_shaker_parameters(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Waveform, float Amp, float Freq, float Fade_time)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data[0] = Waveform;
	Ops.floatData[0] = Amp;
	Ops.floatData[1] = Freq;
	Ops.floatData[2] = Fade_time;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_SHAKER_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_aeroforce_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Gradient, float Offset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Gradient;
	Ops.floatData[1] = Offset;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AEROFORCE_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_host_input_force(uint16_t DeviceNo, uint16_t AxisNo, float Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Force;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_HOST_INPUT_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_offset(uint16_t DeviceNo, uint16_t AxisNo, float Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Force;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_OFFSET,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_force(uint16_t DeviceNo, uint16_t AxisNo, float* Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(Force + i) = RetData.FloatValue_Multi[i];
			}
		else
			*Force = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_link_state(uint16_t DeviceNo, uint16_t* State)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_LINK_STATE,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
		* State = RetData.U16Value;

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_sensor_direction(uint16_t DeviceNo, uint16_t AxisNo, float Dir)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Dir;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_SENSOR_DIRECTION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ap_trim_switch(uint16_t DeviceNo, uint16_t AxisNo, uint16_t m_switch)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = m_switch;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AP_TRIM_SWITCH,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ap_velocity(uint16_t DeviceNo, uint16_t AxisNo, float Velocity)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Velocity;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AP_VELOCITY,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_trim_velocity(uint16_t DeviceNo, uint16_t AxisNo, float Velocity)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Velocity;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_VELOCITY,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_trim_init_position(uint16_t DeviceNo, uint16_t AxisNo, float Position)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Position;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TRIM_INIT_POSITION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ap_force_override(uint16_t DeviceNo, uint16_t AxisNo, float Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Force;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AP_FORCE_OVERRIDE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ap_trim_switch_parameters(uint16_t DeviceNo, uint16_t AxisNo, uint32_t m_switch, float AP_Vel, float Trim_Vel, float Trim_initpos, float AP_override)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data[0] = m_switch;
	Ops.floatData[0] = AP_Vel;
	Ops.floatData[1] = Trim_Vel;
	Ops.floatData[2] = Trim_initpos;
	Ops.floatData[3] = AP_override;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AP_TRIM_SWITCH_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_calculated_force(uint16_t DeviceNo, uint16_t AxisNo, float* Force)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_CALCULATED_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(Force + i) = RetData.FloatValue_Multi[i];
			}
		else
			*Force = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_StMove_trim_position(uint16_t DeviceNo, uint16_t AxisNo, float Position, uint32_t Steps)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data[0] = Steps;
	Ops.floatData[0] = Position;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_MOVE_TRIM_POSITION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_ai(uint16_t DeviceNo, uint16_t AxisNo, float* Ai)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_AI,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(Ai + i) = RetData.FloatValue_Multi[i];
			}
		else
			*Ai = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisMaxVelocity(uint16_t DeviceNo, uint16_t AxisNo, double MaxVelocity)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = MaxVelocity;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_MAX_VELOCITY,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisMaxVelocity(uint16_t DeviceNo, uint16_t AxisNo, double *MaxVelocity)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_MAX_VELOCITY,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*MaxVelocity = RetData.DoubleValue[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbs_P2P(uint16_t DeviceNo, uint16_t GroupNo, double Pos[])
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_P2P, true, 0, 0, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineRel_P2P(uint16_t DeviceNo, uint16_t GroupNo, double Pos[])
{
	return SendMcGroupMoveLine(DeviceNo, GroupNo, MC_LINE_MODE_P2P, false, 0, 0, Pos, NULL);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveAbs_P2P(uint16_t DeviceNo, uint16_t AxisNo, double Pos)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_ABS_P2P,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveRel_P2P(uint16_t DeviceNo, uint16_t AxisNo, double Pos)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Pos;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_REL_P2P,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
void Cal_ECAT2016N_Scale(ecat_slave_aio_config_t *AioConfig)
{
	switch (AioConfig->Range)
	{
	case ECAT2016_AI_BI_1_25V:
		AioConfig->VoltScaling = 1.25 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_600mV:
		AioConfig->VoltScaling = 0.6 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_300mV:
		AioConfig->VoltScaling = 0.3 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_125mV:
		AioConfig->VoltScaling = 0.125 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_80mV:
		AioConfig->VoltScaling = 0.08 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_60mV:
		AioConfig->VoltScaling = 0.06 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_40mV:
		AioConfig->VoltScaling = 0.04 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_30mV:
		AioConfig->VoltScaling = 0.03 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_20mV:
		AioConfig->VoltScaling = 0.02 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_15mV:
		AioConfig->VoltScaling = 0.015 / 0x7FFF;
		break;
	case ECAT2016_AI_BI_10mV:
		AioConfig->VoltScaling = 0.01 / 0x7FFF;
		break;
	}
}
void Cal_ECAT2016_3_Scale(ecat_slave_aio_config_t* AioConfig)
{
	switch (AioConfig->Range)
	{
	case ECAT2016_3_AI_BI_10V:
		AioConfig->VoltScaling = 10 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_5V:
		AioConfig->VoltScaling = 5 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_2_5V:
		AioConfig->VoltScaling = 2.5 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_1_25V:
		AioConfig->VoltScaling = 1.25 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_625mV:
		AioConfig->VoltScaling = 0.625 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_312mV:
		AioConfig->VoltScaling = 0.312 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_200mV:
		AioConfig->VoltScaling = 0.2 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_100mV:
		AioConfig->VoltScaling = 0.1 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_50mV:
		AioConfig->VoltScaling = 0.05 / 0x7FFF;
		break;
	case ECAT2016_3_AI_BI_25mV:
		AioConfig->VoltScaling = 0.025 / 0x7FFF;
		break;
	}
}
/*****************************************************************************/
int32_t ECAT_Set_ECAT2016_AiProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t Range, uint32_t *AbortCode)
{
    int32_t ret;
    uint16_t Index;
	uint32_t ObjectVal;
	uint8_t is2016n = 0;
	if (Range > ECAT2016_AI_BI_10mV)
		return ECAT_ERR_INVALID_PARAM;
	//Revision No
	ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, 0x1018, 0x03, 4, &ObjectVal, AbortCode);
	if (ret == 0)
	{
		if (ObjectVal == 0x00100001)
			is2016n = 1;
		Index = 0x8000 + (0x10 * ChannelNo);
		ret = ECAT_SetSlaveSdoObject(DeviceNo, SlaveNo, Index, 0x19, 4, Range, AbortCode);
		if(ret == 0)
		{
			uint16_t Pos;
			ret = ECAT_GetSlavePos(DeviceNo, SlaveNo, &Pos);
			if (ret == 0)
			{
				ecat_slave_aio_config_t* AioConfig = &EcatDevice[DeviceNo].Slave[Pos].AioConfig;
				AioConfig->Range = Range;
				if (is2016n)
					Cal_ECAT2016N_Scale(AioConfig);
				else
					Cal_ECAT2016_3_Scale(AioConfig);
			}
		}
	}
    return ret;
}
/*****************************************************************************/
int32_t ECAT_Get_ECAT2016_AiProperty(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, uint8_t *Range, uint32_t *AbortCode)
{
	int32_t ret;
	uint16_t Index;
	uint32_t ObjectVal;
	uint8_t is2016n = 0;
	//Revision No
	ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, 0x1018, 0x03, 4, &ObjectVal, AbortCode);
	if (ret == 0)
	{
		if (ObjectVal == 0x00100001)
			is2016n = 1;
		Index = 0x8000 + (0x10 * ChannelNo);
		ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, Index, 0x19, 4, &ObjectVal, AbortCode);
		if (ret == 0)
		{
			uint16_t Pos;
			ret = ECAT_GetSlavePos(DeviceNo, SlaveNo, &Pos);
			if (ret == 0)
			{
				ecat_slave_aio_config_t* AioConfig = &EcatDevice[DeviceNo].Slave[Pos].AioConfig;
				AioConfig->Range = ObjectVal;
				if (is2016n)
					Cal_ECAT2016N_Scale(AioConfig);
				else
					Cal_ECAT2016_3_Scale(AioConfig);
				*Range = ObjectVal;
			}
		}
	}
    return ret;
}
/*****************************************************************************/
int32_t ECAT_Get_ECAT2016_AiRawData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, int16_t *Data)
{
  	int32_t ret;
    ecat_2016_t Data_st;
    ret = ECAT_GetSlaveTxPdoData(DeviceNo,  SlaveNo, sizeof(ecat_2016_t) * ChannelNo, sizeof(ecat_2016_t), (uint8_t *)&Data_st);
    if(ret == 0)
        *Data = Data_st.Value;
    else if(ret == ECAT_ERR_INVALID_OFFSET)
        ret = ECAT_ERR_INVALID_CHANNEL;
    return ret;
}
/*****************************************************************************/
int32_t ECAT_Get_ECAT2016_AiVoltData(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelNo, double *Data)
{
    CHECK_ECAT_SLAVE_NO(SlaveNo);
  	int32_t ret;
    ecat_2016_t Data_st;
    ret = ECAT_GetSlaveTxPdoData(DeviceNo,  SlaveNo, sizeof(ecat_2016_t) * ChannelNo, sizeof(ecat_2016_t), (uint8_t *)&Data_st);
    if(ret == 0)
    {
		uint16_t Pos;
		ret = ECAT_GetSlavePos(DeviceNo, SlaveNo, &Pos);
		if (ret == 0)
		{
			ecat_slave_aio_config_t* AioConfig = &EcatDevice[DeviceNo].Slave[Pos].AioConfig;
			*Data = Data_st.Value * AioConfig->VoltScaling;
		}
    }
    else if(ret == ECAT_ERR_INVALID_OFFSET)
        ret = ECAT_ERR_INVALID_CHANNEL;
    return ret;
}
/*****************************************************************************/
int32_t ECAT_EvSetCompareAiParameters(uint16_t DeviceNo, uint16_t EventID, uint16_t Operator, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, double ScaleGain, double ScaleOffset, double CompareValue)
{
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.SlaveNo= SlaveNo;
	Ops.U16Data[0] = Operator;
	Ops.U16Data[1] = OffsetByte;
	Ops.U32Data[0] = DataSize;
	Ops.DoubleData[0] = ScaleGain;
	Ops.DoubleData[1] = ScaleOffset;
	Ops.DoubleData[2] = CompareValue;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_COMPARE_AI_PARAMETERS,
							&Ops, sizeof(ecat_event_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McSetAxisTorqueGain(uint16_t DeviceNo, uint16_t AxisNo, double Gain, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Gain;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_SET_AXIS_TORQUE_GAIN,
							&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret != 0) {
		*AbortCode = RetData.U32Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisTorqueGain(uint16_t DeviceNo, uint16_t AxisNo, double *Gain)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_TORQUE_GAIN,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Gain = RetData.DoubleValue[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisRatedTorque(uint16_t DeviceNo, uint16_t AxisNo, double *Torque, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_RATED_TORQUE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
        *Torque = RetData.DoubleValue[0];
    else
		*AbortCode = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveTor(uint16_t DeviceNo, uint16_t AxisNo, double Torque)
{
	ecat_mc_axis_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Torque;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_TOR,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualTorque(uint16_t DeviceNo, uint16_t AxisNo, double *Torque)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACTUAL_TORQUE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Torque + i) = RetData.DoubleValue_multi[i];
		} else
			*Torque = RetData.DoubleValue_multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisMaxTorque(uint16_t DeviceNo, uint16_t AxisNo, double Torque, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.AxisNo = AxisNo;
    Ops.DoubleData[0] = Torque;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_MAX_TORQUE,
							&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret != 0) {
		*AbortCode = RetData.U32Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisMaxTorque(uint16_t DeviceNo, uint16_t AxisNo, double *Torque, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_MAX_TORQUE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
        *Torque = RetData.DoubleValue[0];
    else
		*AbortCode = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisCommandVel(uint16_t DeviceNo, uint16_t AxisNo, double *Vel)
{
	int32_t ret;
	uint16_t i;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_CMD_VEL,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0) {
		if (AxisNo == 65535) {
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
				*(Vel + i) = RetData.DoubleValue_multi[i];
		} else
			*Vel = RetData.DoubleValue_multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxesCommandPos(uint16_t DeviceNo, uint16_t GroupNo, double* Pos, uint16_t Arr)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXES_CMD_POS,
		&Ops, sizeof(ecat_mc_group_ops_t), &Ops, sizeof(ecat_mc_group_ops_t));
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
			if (Arr == EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
			memcpy(Pos, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		else if (Arr > EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
		{
			uint16_t AxesCnt = EcatDevice[DeviceNo].GrpAxisCnt[GroupNo];

			memset(&Pos[AxesCnt], 0, sizeof(double) * (Arr - AxesCnt));
			memcpy(Pos, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		}
		else
		{
			memcpy(Pos, Ops.DoubleData, sizeof(double) * Arr);
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxesCommandVel(uint16_t DeviceNo, uint16_t GroupNo, double* Vel, uint16_t Arr)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXES_CMD_VEL,
		&Ops, sizeof(ecat_mc_group_ops_t), &Ops, sizeof(ecat_mc_group_ops_t));
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (Arr == EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
			memcpy(Vel, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		else if (Arr > EcatDevice[DeviceNo].GrpAxisCnt[GroupNo])
		{
			uint16_t AxesCnt = EcatDevice[DeviceNo].GrpAxisCnt[GroupNo];
			memset(&Vel[AxesCnt], 0, sizeof(double) * (Arr - AxesCnt));
			memcpy(Vel, Ops.DoubleData, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
		}
		else
		{
			memcpy(Vel, Ops.DoubleData, sizeof(double) * Arr);
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}

	return ret;
}
/*****************************************************************************/
int32_t SendMovePathCtrlAction(uint16_t DeviceNo, uint8_t Read, ecat_mc_move_path_ops_t *Ops)
{
	if (Read) {
		return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_PATH_CTRL_ACTION,
								Ops, sizeof(ecat_mc_move_path_ops_t), Ops, sizeof(ecat_mc_move_path_ops_t));
	} else {
		return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_PATH_CTRL_ACTION,
								Ops, sizeof(ecat_mc_move_path_ops_t), NULL, 0);
	}
}
/*****************************************************************************/
int32_t ECAT_McGroupMovePath(uint16_t DeviceNo, uint16_t GroupNo, uint16_t PathDataNo, uint8_t Restart, uint16_t DataIndex, uint8_t Repeat)
{
	ecat_mc_move_path_ops_t Ops;
	Ops.GroupNo = GroupNo;
	Ops.PathDataNo = PathDataNo;
	Ops.DataIndex = DataIndex;
	Ops.Restart = Restart;
	Ops.Repeat = Repeat;
	Ops.ActionType = MC_MPCA_MOVING;
	return SendMovePathCtrlAction(DeviceNo, 0, &Ops);
}
/*****************************************************************************/
int32_t ECAT_McAddPathData(uint16_t DeviceNo, uint16_t PathDataNo, uint32_t CmdType,
		uint8_t AbsMove, double EndPos[], double AuxPos[], double Args[])
{
	ecat_mc_move_path_ops_t Ops;
	Ops.PathDataNo = PathDataNo;
	Ops.Queue.CmdType = CmdType;
	Ops.Queue.AbsMove = AbsMove;
	Ops.ActionType = MC_MPCA_ADD_DATA;
	if (EndPos)
		memcpy(Ops.Queue.EndPos, EndPos, sizeof(Ops.Queue.EndPos));
	if (AuxPos)
		memcpy(Ops.Queue.AuxPos, AuxPos, sizeof(Ops.Queue.AuxPos));
	if (Args)
		memcpy(Ops.Queue.Args, Args, sizeof(Ops.Queue.Args));
	return SendMovePathCtrlAction(DeviceNo, 0, &Ops);
}
/*****************************************************************************/
int32_t ECAT_McSetPathData(uint16_t DeviceNo, uint16_t PathDataNo, uint16_t DataIndex, uint32_t CmdType,
		uint8_t AbsMove, double EndPos[], double AuxPos[], double Args[])
{
	ecat_mc_move_path_ops_t Ops;
	Ops.PathDataNo = PathDataNo;
	Ops.DataIndex = DataIndex;
	Ops.Queue.CmdType = CmdType;
	Ops.Queue.AbsMove = AbsMove;
	Ops.ActionType = MC_MPCA_SET_DATA;
	if (EndPos)
		memcpy(Ops.Queue.EndPos, EndPos, sizeof(Ops.Queue.EndPos));
	if (AuxPos)
		memcpy(Ops.Queue.AuxPos, AuxPos, sizeof(Ops.Queue.AuxPos));
	if (Args)
		memcpy(Ops.Queue.Args, Args, sizeof(Ops.Queue.Args));
	return SendMovePathCtrlAction(DeviceNo, 0, &Ops);
}
/*****************************************************************************/
int32_t ECAT_McGetPathData(uint16_t DeviceNo, uint16_t PathDataNo, uint16_t DataIndex, uint32_t *CmdType,
		uint8_t *AbsMove, double EndPos[], double AuxPos[], double Args[])
{
	ecat_mc_move_path_ops_t Ops;
	int32_t ret;
	Ops.PathDataNo = PathDataNo;
	Ops.DataIndex = DataIndex;
	Ops.ActionType = MC_MPCA_GET_DATA;
	ret = SendMovePathCtrlAction(DeviceNo, 1, &Ops);
	if (ret == 0) {
		*CmdType = Ops.Queue.CmdType;
		*AbsMove = Ops.Queue.AbsMove;
		if (EndPos)
			memcpy(EndPos, Ops.Queue.EndPos, sizeof(Ops.Queue.EndPos));
		if (AuxPos)
			memcpy(AuxPos, Ops.Queue.AuxPos, sizeof(Ops.Queue.AuxPos));
		if (Args)
			memcpy(Args, Ops.Queue.Args, sizeof(Ops.Queue.Args));
	}

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McClearPathData(uint16_t DeviceNo, uint16_t PathDataNo)
{
	ecat_mc_move_path_ops_t Ops;
	Ops.PathDataNo = PathDataNo;
	Ops.ActionType = MC_MPCA_CLEAR_DATA;
	return SendMovePathCtrlAction(DeviceNo, 0, &Ops);
}
/*****************************************************************************/
int32_t ECAT_McGetPathDataSize(uint16_t DeviceNo, uint16_t PathDataNo, uint16_t *Size)
{
	ecat_mc_move_path_ops_t Ops;
	int32_t ret;
	Ops.PathDataNo = PathDataNo;
	Ops.ActionType = MC_MPCA_GET_DATA_SIZE;
	ret = SendMovePathCtrlAction(DeviceNo, 1, &Ops);
	if (ret == 0)
		*Size = Ops.DataIndex;

	return ret;
}
/*****************************************************************************/
int32_t SendMcMoveAdv(uint16_t DeviceNo, ecat_mc_move_adv_t *Ops, uint8_t AccDecMode)
{
	if (AccDecMode >= MC_ACC_DEC_MODE_TIME) {
		Ops->Accel = fabs((Ops->ReqVel - Ops->StartVel) / Ops->Accel);
		Ops->Decel = fabs((Ops->ReqVel - Ops->FinalVel) / Ops->Decel);
	}

	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_ADV,
							Ops, sizeof(ecat_mc_move_adv_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveAbsAdv(uint16_t DeviceNo, uint16_t AxisNo, double EndPos,
		double StartVel, double ReqVel, double FinalVel, double Accel, double Decel, uint8_t AccDecMode)
{
	ecat_mc_move_adv_t Ops;
	Ops.No = AxisNo;
	Ops.GroupMove = 0;
	Ops.AbsMove = 1;
	Ops.StartVel = StartVel;
	Ops.ReqVel = ReqVel;
	Ops.FinalVel = FinalVel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	Ops.EndPos[0] = EndPos;
	return SendMcMoveAdv(DeviceNo, &Ops, AccDecMode);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveRelAdv(uint16_t DeviceNo, uint16_t AxisNo, double EndPos,
		double StartVel, double ReqVel, double FinalVel, double Accel, double Decel, uint8_t AccDecMode)
{
	ecat_mc_move_adv_t Ops;
	Ops.No = AxisNo;
	Ops.GroupMove = 0;
	Ops.AbsMove = 0;
	Ops.StartVel = StartVel;
	Ops.ReqVel = ReqVel;
	Ops.FinalVel = FinalVel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	Ops.EndPos[0] = EndPos;
	return SendMcMoveAdv(DeviceNo, &Ops, AccDecMode);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineAbsAdv(uint16_t DeviceNo, uint16_t GroupNo, double EndPos[],
		double StartVel, double ReqVel, double FinalVel, double Accel, double Decel, uint8_t AccDecMode)
{
	ecat_mc_move_adv_t Ops;
	Ops.No = GroupNo;
	Ops.GroupMove = 1;
	Ops.AbsMove = 1;
	Ops.StartVel = StartVel;
	Ops.ReqVel = ReqVel;
	Ops.FinalVel = FinalVel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.EndPos, EndPos, sizeof(double)*EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return SendMcMoveAdv(DeviceNo, &Ops, AccDecMode);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLineRelAdv(uint16_t DeviceNo, uint16_t GroupNo, double EndPos[],
		double StartVel, double ReqVel, double FinalVel, double Accel, double Decel, uint8_t AccDecMode)
{
	ecat_mc_move_adv_t Ops;
	Ops.No = GroupNo;
	Ops.GroupMove = 1;
	Ops.AbsMove = 0;
	Ops.StartVel = StartVel;
	Ops.ReqVel = ReqVel;
	Ops.FinalVel = FinalVel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.EndPos, EndPos, sizeof(double)*EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return SendMcMoveAdv(DeviceNo, &Ops, AccDecMode);
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_parameters(uint16_t DeviceNo, uint16_t AxisNo, float* Data, uint16_t Size)
{
	int32_t ret;
	ecat_mc_stick_pra_ops_t Ops;
	if (Size > ECAT_IPC_32_DATA_SIZE_MAX)
		Size = ECAT_IPC_32_DATA_SIZE_MAX;
	Ops.AxisNo = AxisNo;
	memcpy(Ops.FData, Data, Size);
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_pra_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StGet_axis_parameters(uint16_t DeviceNo, uint16_t AxisNo, float* Data, uint16_t Size)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_pra_return_t RetData;
	Ops.AxisNo = AxisNo;
	if (Size > ECAT_IPC_32_DATA_SIZE_MAX)
		Size = ECAT_IPC_32_DATA_SIZE_MAX;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_pra_return_t));
	if (ret == 0)
		memcpy(Data, RetData.FData, Size);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_gradient_multiplier_ex(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float Multiplier)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.TableNo = TableNo;
	Ops.floatData[0] = Multiplier;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_MULTIPLIER_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_gradient_zone_ratio_ex(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, uint16_t r1, uint16_t r2, uint16_t r3, uint16_t n1, uint16_t n2, uint16_t n3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.TableNo = TableNo;
	Ops.U16Data[0] = r1;
	Ops.U16Data[1] = r2;
	Ops.U16Data[2] = r3;
	Ops.U16Data[3] = n1;
	Ops.U16Data[4] = n2;
	Ops.U16Data[5] = n3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_ZONE_RATIO_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_gradient_positive_ex(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float g0, float g1, float g2, float g3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.TableNo = TableNo;
	Ops.floatData[0] = g0;
	Ops.floatData[1] = g1;
	Ops.floatData[2] = g2;
	Ops.floatData[3] = g3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_POSITIVE_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_gradient_negative_ex(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float g0, float g1, float g2, float g3)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.TableNo = TableNo;
	Ops.floatData[0] = g0;
	Ops.floatData[1] = g1;
	Ops.floatData[2] = g2;
	Ops.floatData[3] = g3;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_NEGATIVE_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_StSet_axis_car_speed(uint16_t DeviceNo, uint16_t AxisNo, float Value)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Value;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GRADIENT_NEGATIVE_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_coulomb_force_ex(uint16_t DeviceNo, uint16_t AxisNo, float GradientPos, float OffsetPos, float GradientNeg, float OffsetNeg, float VelocityZone)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = GradientPos;
	Ops.floatData[1] = OffsetPos;
	Ops.floatData[2] = GradientNeg;
	Ops.floatData[3] = OffsetNeg;
	Ops.floatData[4] = VelocityZone;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_COULOMB_FORCE_EX,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_force_filter_cutoff_freq(uint16_t DeviceNo, uint16_t AxisNo, float Frequency)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Frequency;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FORCE_FILTER_CITOFF_FREQ,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Backlash_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Mass_b, float B_b, float MaxAcc_b, float MaxVel_b, float Coulomb_b, float Backlash)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass_b;
	Ops.floatData[1] = B_b;
	Ops.floatData[2] = MaxAcc_b;
	Ops.floatData[3] = MaxVel_b;
	Ops.floatData[4] = Coulomb_b;
	Ops.floatData[5] = Backlash;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BACKLASH_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Backlash_Enable(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Enable)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.U32Data[0] = Enable;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BACKLASH_ENABLE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_model_X(uint16_t DeviceNo, uint16_t AxisNo, float* X)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_MODEL_X,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(X + i) = RetData.FloatValue_Multi[i];
			}
		else
			*X = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisMove_CiA402_PP(uint16_t DeviceNo, uint16_t AxisNo, uint8_t Abort, uint8_t AbsMove,
		double EndPos, double Vel, double Accel, double Decel)
{
	ecat_mc_move_cia402_profile_t Ops;
	Ops.No = AxisNo;
	Ops.Mode = CIA402_PROFILE_POSITION;
	Ops.Abort = Abort;
	Ops.AbsMove = AbsMove;
	Ops.Vel = Vel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	Ops.EndPos = EndPos;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_CIA402_PROFILE_MODE,
							&Ops, sizeof(ecat_mc_move_cia402_profile_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMove_CiA402_PV(uint16_t DeviceNo, uint16_t AxisNo, double Vel, double Accel, double Decel)
{
	ecat_mc_move_cia402_profile_t Ops;
	Ops.No = AxisNo;
	Ops.Mode = CIA402_PROFILE_VOLOCITY;
	Ops.Vel = Vel;
	Ops.Accel = Accel;
	Ops.Decel = Decel;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_CIA402_PROFILE_MODE,
							&Ops, sizeof(ecat_mc_move_cia402_profile_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMove_CiA402_PT(uint16_t DeviceNo, uint16_t AxisNo, double Torque, double Slope)
{
	ecat_mc_move_cia402_profile_t Ops;
	Ops.No = AxisNo;
	Ops.Mode = CIA402_PROFILE_TORQUE;
	Ops.EndPos = Torque;
	Ops.Accel = Slope;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_MOVE_CIA402_PROFILE_MODE,
							&Ops, sizeof(ecat_mc_move_cia402_profile_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMove_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint8_t BufferMode, uint8_t AbsMove,
	double Pos, double Vel, uint16_t AccTime, uint8_t AccDecType, uint32_t* CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	Ops.No = AxisNo;
	Ops.AbsMove = AbsMove;
	Ops.Pos[0] = Pos;
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.AccDecType = AccDecType;
	Ops.BufferMode = BufferMode;
	Ops.Type = MC_PLC_AT_AXIS_MOVE;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisCmdState_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint32_t CmdIndex, uint32_t* State)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = AxisNo;
	Ops.CmdIndex = CmdIndex;
	Ops.Type = MC_PLC_ST_AXIS_CMD_STATE;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if ((ret == 0) && (State))
		* State = Ops.u32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupCmdState_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint32_t CmdIndex, uint32_t* State)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = GroupNo;
	Ops.CmdIndex = CmdIndex;
	Ops.Type = MC_PLC_ST_GROUP_CMD_STATE;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if ((ret == 0) && (State))
		* State = Ops.u32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxesServoOn(uint16_t DeviceNo, uint16_t GroupNo, uint32_t* State)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = GroupNo;
	Ops.Type = MC_PLC_ST_GROUP_SERVO_ON;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if ((ret == 0) && (State))
		* State = Ops.u32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisStates_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint32_t * State, int32_t * LastError, int16_t * DriveError)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = AxisNo;
	Ops.Type = MC_PLC_ST_AXIS_STATES;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if (ret == 0) {
		if (State)
			* State = Ops.u32Value;
		if (LastError)
			* LastError = Ops.i32Value;
		if (DriveError)
			* DriveError = Ops.i16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisCmdBufferMax_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Max)
{
	ecat_mc_plc_data_t Ops;
	Ops.No = AxisNo;
	Ops.u32Value = Max;
	Ops.Type = MC_PLC_AT_SET_AXIS_CMD_BUFFER_MAX;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveVel_PLC(uint16_t DeviceNo, uint16_t AxisNo, double Vel, uint16_t AccTime, uint32_t * CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	Ops.No = AxisNo;
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.Type = MC_PLC_AT_AXIS_MOVE_VEL;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisHome_PLC(uint16_t DeviceNo, uint16_t AxisNo, int32_t Method,
	double Offset, double SeachSw, double SeachZr, double Acc)
{
	ecat_mc_plc_data_t Ops;
	Ops.No = AxisNo;
	Ops.Pos[0] = Offset;
	Ops.i32Value = Method;
	Ops.Vel[0] = SeachSw;
	Ops.Vel[1] = SeachZr;
	Ops.Acc = Acc;
	Ops.Type = MC_PLC_AT_AXIS_HOME;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetGroupStates_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint32_t * State, int32_t * LastError, int16_t * DriveError)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = GroupNo;
	Ops.Type = MC_PLC_ST_GROUP_STATES;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if (ret == 0) {
		if (State)
			* State = Ops.u32Value;
		if (LastError)
			* LastError = Ops.i32Value;
		if (DriveError)
			* DriveError = Ops.i16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisGroupStates_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint16_t * GroupNo, uint32_t * State)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = AxisNo;
	Ops.Type = MC_PLC_ST_AXIS_GROUP_STATES;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if (ret == 0) {
		if (GroupNo)
			* GroupNo = Ops.No;
		if ((*GroupNo != 0xFFFF) && State)
			* State = Ops.u32Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupCmdBufferMax_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint32_t Max)
{
	ecat_mc_plc_data_t Ops;
	Ops.No = GroupNo;
	Ops.u32Value = Max;
	Ops.Type = MC_PLC_AT_SET_GRP_CMD_BUFFER_MAX;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveLine_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint8_t BufferMode, uint8_t AbsMove,
	double Pos[], uint32_t ArraySize, double Vel, uint16_t AccTime, uint8_t AccDecType, uint16_t BlendPercent, uint32_t * CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	Ops.No = GroupNo;
	Ops.BufferMode = BufferMode;
	Ops.AbsMove = AbsMove;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (ArraySize < EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]) {
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		return ECAT_ERR_INVALID_DATA_SIZE;
	}
	memcpy(Ops.Pos, Pos, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.AccDecType = AccDecType;
	Ops.u32Value = BlendPercent;
	Ops.Type = MC_PLC_AT_GRP_MOVE_LINE;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveCircular_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint8_t BufferMode, uint8_t AbsMove,
	uint8_t CircMode, uint8_t Dir, double Angle, double AuxPos[], double EndPos[], uint32_t ArraySize, double Vel,
	uint16_t AccTime, uint8_t AccDecType, uint16_t BlendPercent, uint32_t * CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	Ops.No = GroupNo;
	Ops.BufferMode = BufferMode;
	Ops.AbsMove = AbsMove;
	if (CircMode == 0)
		Ops.CircMode = MC_CIRC_MODE_BP_EP;
	else if (CircMode == 1)
		Ops.CircMode = MC_CIRC_MODE_CP_EP;
	else
		Ops.CircMode = MC_CIRC_MODE_CP_ANGLE;
	Ops.Dir = Dir;
	Ops.Angle = Angle;
	memcpy(Ops.AuxPos, AuxPos, sizeof(Ops.AuxPos));
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (ArraySize < EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]) {
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		return ECAT_ERR_INVALID_DATA_SIZE;
	}
	memcpy(Ops.Pos, EndPos, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.AccDecType = AccDecType;
	Ops.u32Value = BlendPercent;
	Ops.Type = MC_PLC_AT_GRP_MOVE_CIRC;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveHelix_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint8_t BufferMode, uint8_t AbsMove,
	double Angle, double Pitch, double AuxPos[], double NV[], uint32_t ArraySize, double Vel,
	uint16_t AccTime, uint8_t AccDecType, uint16_t BlendPercent, uint32_t * CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	if (ArraySize < 3)
		return ECAT_ERR_INVALID_DATA_SIZE;

	Ops.No = GroupNo;
	Ops.BufferMode = BufferMode;
	Ops.AbsMove = AbsMove;
	Ops.Angle = Angle;
	Ops.Pitch = Pitch;
	memcpy(Ops.AuxPos, AuxPos, sizeof(Ops.AuxPos));
	memcpy(Ops.Pos, NV, sizeof(double) * 3);
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.AccDecType = AccDecType;
	Ops.u32Value = BlendPercent;
	Ops.Type = MC_PLC_AT_GRP_MOVE_HELIX;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveConicalHelix_PLC(uint16_t DeviceNo, uint16_t GroupNo, uint8_t BufferMode, uint8_t AbsMove,
	double Angle, double Pitch, double EndRadius, double AuxPos[], double NV[], uint32_t ArraySize, double Vel,
	uint16_t AccTime, uint8_t AccDecType, uint16_t BlendPercent, uint32_t * CmdIndex)
{
	int32_t ret;
	ecat_mc_plc_data_t Ops;
	uint32_t cmd_idx;
	if (ArraySize < 3)
		return ECAT_ERR_INVALID_DATA_SIZE;

	Ops.No = GroupNo;
	Ops.BufferMode = BufferMode;
	Ops.AbsMove = AbsMove;
	Ops.Angle = Angle;
	Ops.Pitch = Pitch;
	Ops.EndRadius = EndRadius;
	memcpy(Ops.AuxPos, AuxPos, sizeof(Ops.AuxPos));
	memcpy(Ops.Pos, NV, sizeof(double) * 3);
	Ops.Vel[0] = Vel;
	Ops.AccTime = AccTime;
	Ops.AccDecType = AccDecType;
	Ops.u32Value = BlendPercent;
	Ops.Type = MC_PLC_AT_GRP_MOVE_CONICAL_HELIX;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ACTION_PLC,
		&Ops, sizeof(ecat_mc_plc_data_t), &cmd_idx, sizeof(uint32_t));
	if ((ret == 0) && (CmdIndex))
		* CmdIndex = cmd_idx;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisInfo_PLC(uint16_t DeviceNo, uint16_t AxisNo, uint32_t * DI, uint32_t * AlStates, uint16_t * HomeState,
	uint16_t * SoftLimitState)
{
	int32_t ret;
	ecat_mc_plc_state_t Ops;
	Ops.No = AxisNo;
	Ops.Type = MC_PLC_ST_AXIS_INFO;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_STATES_PLC,
		&Ops, sizeof(ecat_mc_plc_state_t), &Ops, sizeof(ecat_mc_plc_state_t));
	if (ret == 0) {
		if (DI)
			* DI = Ops.u32Value;
		if (AlStates)
			* AlStates = Ops.i16Value;
		if (HomeState)
			* HomeState = Ops.u16Value[0];
		if (SoftLimitState)
			* SoftLimitState = Ops.u16Value[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetTxPdoBufParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, uint16_t Enable)
{
	ecat_pdo_rec_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.No = SlaveNo;
    Ops.OffsetByte = OffsetByte;
    Ops.DataSize = DataSize;
    Ops.Enable = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_TX_BUFFER_PARAM,
							&Ops, sizeof(ecat_pdo_rec_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetTxPdoBufParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *SlaveNo, uint16_t *OffsetByte, uint16_t *DataSize, uint16_t *Enable)
{
	int32_t ret;
	ecat_pdo_rec_ops_t Ops;
	Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TX_BUFFER_PARAM,
							&Ops, sizeof(ecat_pdo_rec_ops_t), &Ops, sizeof(ecat_pdo_rec_ops_t));
	if (ret == 0) {
        *SlaveNo = Ops.No;
        *OffsetByte = Ops.OffsetByte;
        *DataSize = Ops.DataSize;
        *Enable = Ops.Enable;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetTxPdoBufEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t Enable)
{
	ecat_pdo_rec_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.Enable = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_TX_BUFFER_ENABLE,
							&Ops, sizeof(ecat_pdo_rec_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetTxPdoBufEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *Enable)
{
	int32_t ret;
	ecat_pdo_rec_ops_t Ops;
	Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TX_BUFFER_ENABLE,
							&Ops, sizeof(ecat_pdo_rec_ops_t), &Ops, sizeof(ecat_pdo_rec_ops_t));
	if (ret == 0) {
		*Enable = Ops.Enable;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_GetTxPdoBufValue(uint16_t DeviceNo, uint16_t ChannelNo, float *Data, uint16_t Size, uint16_t *ActualGetSize)
{
	int32_t ret;
	ecat_pdo_rec_ops_t Ops;
	ecat_pdo_rec_return_t RetData;
	Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_TX_BUFFER_VALUE,
							&Ops, sizeof(ecat_pdo_rec_ops_t), &RetData, sizeof(ecat_pdo_rec_return_t));
	if (ret == 0) {
        *ActualGetSize = RetData.ActualGetCount;
        if(Size > sizeof(RetData.Data))
        	memcpy(Data, RetData.Data, sizeof(RetData.Data));
        else
        	memcpy(Data, RetData.Data, Size);
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeTorque(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Torque)
{
	ecat_mc_axis_ops_t Ops;
    Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Torque;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_TORQUE,
							&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeTorque(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Torque)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_TORQUE,
							&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0) {
		*Torque = RetData.U16Value;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetAiFilterParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t SlaveNo, uint16_t OffsetByte, uint16_t DataSize, uint16_t Enable)
{
	ecat_ai_filter_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.No = SlaveNo;
    Ops.OffsetByte = OffsetByte;
    Ops.DataSize = DataSize;
    Ops.Enable = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_AI_FILTER_PARAM,
							&Ops, sizeof(ecat_ai_filter_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetAiFilterParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *SlaveNo, uint16_t *OffsetByte, uint16_t *DataSize, uint16_t *Enable)
{
	int32_t ret;
	ecat_ai_filter_ops_t Ops;
	Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_AI_FILTER_PARAM,
							&Ops, sizeof(ecat_ai_filter_ops_t), &Ops, sizeof(ecat_ai_filter_ops_t));
	if (ret == 0) {
        *SlaveNo = Ops.No;
        *OffsetByte = Ops.OffsetByte;
        *DataSize = Ops.DataSize;
        *Enable = Ops.Enable;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetAiFilterEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t Enable)
{
	ecat_ai_filter_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.Enable = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_AI_FILTER_ENABLE,
							&Ops, sizeof(ecat_ai_filter_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetAiFilterEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *Enable)
{
	int32_t ret;
	ecat_ai_filter_ops_t Ops;
	Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_AI_FILTER_ENABLE,
							&Ops, sizeof(ecat_ai_filter_ops_t), &Ops, sizeof(ecat_ai_filter_ops_t));
	if (ret == 0) {
		*Enable = Ops.Enable;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetAiFilterFreq(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t FilterType, double Frequency)
{
	ecat_ai_filter_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.FilterType = FilterType;
    Ops.Frequency = Frequency;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_AI_FILTER_FREQUENCY,
							&Ops, sizeof(ecat_ai_filter_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetAiFilterFreq(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t FilterType, double *Frequency)
{
	int32_t ret;
	ecat_ai_filter_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.FilterType = FilterType;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_AI_FILTER_FREQUENCY,
							&Ops, sizeof(ecat_ai_filter_ops_t), &Ops, sizeof(ecat_ai_filter_ops_t));
	if (ret == 0) {
		*Frequency = Ops.Frequency;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_GetAiFilterOutput(uint16_t DeviceNo, uint16_t ChannelNo, int32_t *Output)
{
	int32_t ret;
	ecat_ai_filter_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_AI_FILTER_OUTPUT,
							&Ops, sizeof(ecat_ai_filter_ops_t), &Ops, sizeof(ecat_ai_filter_ops_t));
	if (ret == 0) {
		*Output = Ops.Output;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetPdoInToOutParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t SlaveNoIn, uint16_t OffsetByteIn, uint16_t DataSizeIn, uint16_t SlaveNoOut, uint16_t OffsetByteOut, uint16_t DataSizeOut)
{
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.No[0] = SlaveNoIn;
    Ops.OffsetByte[0] = OffsetByteIn;
    Ops.No[1] = SlaveNoOut;
    Ops.OffsetByte[1] = OffsetByteOut;
    Ops.DataSize[0] = DataSizeIn;
    Ops.DataSize[1] = DataSizeOut;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PDO_I2O_PARAM,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetPdoInToOutParam(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *SlaveNoIn, uint16_t *OffsetByteIn, uint16_t *DataSizeIn, uint16_t *SlaveNoOut, uint16_t *OffsetByteOut, uint16_t *DataSizeOut)
{
	int32_t ret;
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PDO_I2O_PARAM,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), &Ops, sizeof(ecat_pdo_i2o_ops_t));
	if (ret == 0) {
        *SlaveNoIn = Ops.No[0];
        *OffsetByteIn = Ops.OffsetByte[0];
        *SlaveNoOut = Ops.No[1];
        *OffsetByteOut = Ops.OffsetByte[1];
        *DataSizeIn = Ops.DataSize[0];
        *DataSizeOut = Ops.DataSize[1];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetPdoInToOutCoeff(uint16_t DeviceNo, uint16_t ChannelNo, float gain, float offset)
{
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.gain = gain;
    Ops.offset = offset;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PDO_I2O_COEFF,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetPdoInToOutCoeff(uint16_t DeviceNo, uint16_t ChannelNo, float *gain, float *offset)
{
	int32_t ret;
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PDO_I2O_COEFF,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), &Ops, sizeof(ecat_pdo_i2o_ops_t));
	if (ret == 0) {
        *gain = Ops.gain;
        *offset = Ops.offset;
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_SetPdoInToOutEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t Enable)
{
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
    Ops.Enable = Enable;
	return GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_PDO_I2O_ENABLE,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), NULL, 0);
}
/*****************************************************************************/
int32_t	ECAT_GetPdoInToOutEnable(uint16_t DeviceNo, uint16_t ChannelNo, uint16_t *Enable)
{
	int32_t ret;
	ecat_pdo_i2o_ops_t Ops;
    Ops.ChannelNo = ChannelNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_PDO_I2O_ENABLE,
							&Ops, sizeof(ecat_pdo_i2o_ops_t), &Ops, sizeof(ecat_pdo_i2o_ops_t));
	if (ret == 0) {
        *Enable = Ops.Enable;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGroupMoveShaker(uint16_t DeviceNo, uint16_t GroupNo, double Amp, double phase[], double Freq, double Time)
{
	int32_t ret;
	ecat_mc_group_shaker_ops Ops;
	Ops.GroupNo = GroupNo;
	Ops.Amp = Amp;
	Ops.Freq = Freq;
	Ops.Time = Time;
	Ops.GroupNo = GroupNo;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	memcpy(Ops.phase, phase, sizeof(double) * EcatDevice[DeviceNo].GrpAxisCnt[GroupNo]);
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GROUP_MOVE_SHAKER,
		&Ops, sizeof(ecat_mc_group_shaker_ops), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_STSet_axis_Tor_FeedForward_Gain(uint16_t DeviceNo, uint16_t AxisNo, float Gain)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Gain;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TOR_GEEDFORWARD_GAIN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_STSet_axis_Tor_Compensation(uint16_t DeviceNo, uint16_t AxisNo, float Pos, float Neg, float VelZone)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Pos;
	Ops.floatData[1] = Neg;
	Ops.floatData[2] = VelZone;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TOR_COMPENSATION,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McStewartPlatformShaker(uint16_t DeviceNo, uint16_t GroupNo, double Amp[], double Freq)
{
	int32_t ret;
	ecat_stewart_Shaker_t Ops;

	Ops.GroupNo = GroupNo;
	Ops.time = 2;
	Ops.Freq = Freq;
	memcpy(Ops.Pose, Amp, sizeof(Ops.Pose));

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_TOR_COMPENSATION,
		&Ops, sizeof(ecat_stewart_Shaker_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McStewartPlatformShaker_Stop(uint16_t DeviceNo, uint16_t GroupNo, int QuickStop)
{
	int32_t ret;
	ecat_stewart_Shaker_t Ops;

	Ops.GroupNo = GroupNo;
	Ops.time = QuickStop;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_STEWART_PLATFORM_SHAKER_STOP,
		&Ops, sizeof(ecat_stewart_Shaker_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_cable_model_MZetaWn(uint16_t DeviceNo, uint16_t AxisNo, float Mass, float Zeta, float Wn)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass;
	Ops.floatData[1] = Zeta;
	Ops.floatData[2] = Wn;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_CABLE_MODEL_MZETAWN,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveVelShaker(uint16_t DeviceNo, uint16_t AxisNo, double Amp, double Freq, uint32_t N)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U32Data = N;
	Ops.DoubleData[0] = Amp;
	Ops.DoubleData[1] = Freq;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_VEL_SHAKER,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Fwd_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Mass, float B, float MaxAcc, float MaxVel, float Coulomb, float VelocityZone)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Mass;
	Ops.floatData[1] = B;
	Ops.floatData[2] = MaxAcc;
	Ops.floatData[3] = MaxVel;
	Ops.floatData[4] = Coulomb;
	Ops.floatData[5] = VelocityZone;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_FWD_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_Fwd_model_X(uint16_t DeviceNo, uint16_t AxisNo, float* X)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FWD_MODEL_X,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(X + i) = RetData.FloatValue_Multi[i];
			}
		else
			*X = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_Fwd_model_V(uint16_t DeviceNo, uint16_t AxisNo, float* V)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FWD_MODEL_V,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(V + i) = RetData.FloatValue_Multi[i];
			}
		else
			*V = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_Fwd_model_A(uint16_t DeviceNo, uint16_t AxisNo, float* A)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FWD_MODEL_A,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < MAX_MODEL_NO; i++)
			{
				*(A + i) = RetData.FloatValue_Multi[i];
			}
		else
			*A = RetData.FloatValue_Multi[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Aft_parameters(uint16_t DeviceNo, uint16_t AxisNo, float B, float MaxAcc, float MaxVel)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = B;
	Ops.floatData[1] = MaxAcc;
	Ops.floatData[2] = MaxVel;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AFT_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Cable_parameters(uint16_t DeviceNo, uint16_t AxisNo, float B, float K)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = B;
	Ops.floatData[1] = K;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_CABLE_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Gcompensate_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Maxforce, float DegOffset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = Maxforce;
	Ops.floatData[1] = DegOffset;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_GCOMPENSATE_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryGain(uint16_t DeviceNo, uint16_t SlaveNo, double Kp, double Ki)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = SlaveNo;
	Ops.DoubleData[0] = Kp;
	Ops.DoubleData[1] = Ki;
	Ops.DoubleData[2] = 0;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRY_GAIN,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveAlias(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Alias)
{
	int32_t ret;
	ecat_slave_alias_ops_t Ops;

	Ops.No = SlaveNo;
	Ops.alias = Alias;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_SET_SLAVE_ALIAS,
		&Ops, sizeof(ecat_slave_alias_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveSwAlias(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t* Alias)
{
	int32_t ret;
	ecat_slave_alias_ops_t Ops;
	ecat_slave_alias_ops_t RetData;

	Ops.No = SlaveNo;
	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_GET_SLAVE_SW_ALIAS,
		&Ops, sizeof(ecat_slave_alias_ops_t), &RetData, sizeof(ecat_slave_alias_ops_t));
	if (ret == 0)
		* Alias = RetData.alias;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveNoType(uint16_t DeviceNo, uint16_t Type)
{
	int32_t ret;
	ecat_slave_no_type_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_slave_no_type_ops_t);

	Ops.type = Type;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_SLAVE_NO_TYPE,
		&Ops, sizeof(ecat_slave_no_type_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveNoType(uint16_t DeviceNo, uint16_t* Type)
{
	int32_t ret;
	ecat_slave_no_type_ops_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_NO_TYPE,
		NULL, 0, &RetData, sizeof(ecat_slave_no_type_ops_t));
	if (ret == 0)
		* Type = RetData.type;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlavePos(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t* Pos)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_ipc_base_ops_t RetData;

	Ops.No = SlaveNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_POS,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_ipc_base_ops_t));
	if (ret == 0)
		* Pos = RetData.No;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveNo(uint16_t DeviceNo, uint16_t Pos, uint16_t* SlaveNo)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_ipc_base_ops_t RetData;

	Ops.No = Pos;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_NO,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_ipc_base_ops_t));
	if (ret == 0)
		* SlaveNo = RetData.No;

	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_ai_offset(uint16_t DeviceNo, uint16_t AxisNo, float offset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops;
	Ops.AxisNo = AxisNo;
	Ops.floatData[0] = offset;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AI_OFFSET,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_EvSetMotionCompleteParameters_Grp(uint16_t DeviceNo, uint16_t EventID, uint16_t GrpNo)
{
	int32_t ret;
	ecat_event_ops_t Ops;
	Ops.EventId = EventID;
	Ops.AxisNo = GrpNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_EV_SET_MOTION_COMPLETE_PARAMETERS_GRP,
		&Ops, sizeof(ecat_event_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupPvtDecEnable(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Enable)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Enable;


	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_PVT_DEC_ENABLE,
		&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupPvtDecEnable(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *Enable)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_PVT_DEC_ENABLE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Enable = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Hydraulic_aft_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Mass, float B, float MaxAcc, float MaxVel)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.floatData[0] = Mass;
    Ops.floatData[1] = B;
    Ops.floatData[2] = MaxAcc;
    Ops.floatData[3] = MaxVel;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_HYDRAULIC_AFT_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_valve_parameters(uint16_t DeviceNo, uint16_t AxisNo, float b, float k, float Limit)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.floatData[0] = b;
    Ops.floatData[1] = k;
    Ops.floatData[2] = Limit;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_VALVE_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_boost_gradient_multiplier(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float Multiplier)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.TableNo = TableNo;
    Ops.floatData[0] = Multiplier;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BOOST_GRADIENT_MULTIPLIER,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_boost_gradient_zone_ratio(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, uint16_t r1, uint16_t r2, uint16_t r3)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.TableNo = TableNo;
    Ops.floatData[0] = r1;
    Ops.floatData[1] = r2;
    Ops.floatData[2] = r3;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BOOST_GRADIENT_ZONE_RATIO,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_boost_gradient(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float g0, float g1, float g2, float g3)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.TableNo = TableNo;
    Ops.floatData[0] = g0;
    Ops.floatData[1] = g1;
    Ops.floatData[2] = g2;
    Ops.floatData[3] = g3;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BOOST_GRADIENT,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Aft_model_mode(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Mode)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.U16Data[0] = Mode;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_AFT_MODEL_MODE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Host_Force_Test(uint16_t DeviceNo, uint16_t AxisNo, float Time, float Delay, float ForceMax)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.floatData[0] = Time;
    Ops.floatData[1] = Delay;
    Ops.floatData[2] = ForceMax;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_HOST_FORCE_TEST,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_lateralforce_parameters(uint16_t DeviceNo, uint16_t AxisNo, float Gradient, float Offset)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops ;
	Ops.AxisNo = AxisNo;
    Ops.floatData[0] = Gradient;
    Ops.floatData[1] = Offset;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_LATERALFORCE_PARAMETERS,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_boost_gradient_zone_force(uint16_t DeviceNo, uint16_t AxisNo, uint16_t TableNo, float f1, float f2, float f3, float fmax)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.TableNo = TableNo;
    Ops.floatData[0] = f1;
    Ops.floatData[1] = f2;
    Ops.floatData[2] = f3;
    Ops.floatData[3] = fmax;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_BOOST_GRADIENT_ZONE_FORCE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
    return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_Sim_Pos_SinWave(uint16_t DeviceNo, uint16_t AxisNo,float Amp, float Freq, float time_s)
{
    int32_t ret;
    ecat_mc_stick_ops_t Ops ;
    Ops.AxisNo = AxisNo;
    Ops.floatData[0] = Amp;
    Ops.floatData[1] = Freq;
    Ops.floatData[2] = time_s;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_SIM_POS_SINWAVE,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupCoordinate(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Type)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Type;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_COORDINATE,
		&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_GetDebugMsg(uint16_t DeviceNo, float* data)
{
    int32_t ret;
    ecat_debug_msg_ops_t RetData;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_DEBUG_MSG,
		NULL, 0, &RetData, sizeof(ecat_debug_msg_ops_t));
    memcpy(data, RetData.data, sizeof(float) * 64);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetGroupCoordinateLimit(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Type, uint16_t Enable, double MIN_Value, double MAX_Value)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;

	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Type;
	Ops.U16Data[1] = Enable;
	Ops.DoubleData[0] = MIN_Value;
	Ops.DoubleData[1] = MAX_Value;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_GROUP_COORDINATE_LIMIT,
		&Ops, sizeof(ecat_mc_group_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupCoordinateLimit(uint16_t DeviceNo, uint16_t GroupNo, uint16_t Type, uint16_t *Enable, double *MIN_Value, double *MAX_Value)
{
	int32_t ret;
	ecat_mc_group_ops_t Ops;
    ecat_mc_return_t RetData;

	Ops.GroupNo = GroupNo;
	Ops.U16Data[0] = Type;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_COORDINATE_LIMIT,
		&Ops, sizeof(ecat_mc_group_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
    {
		*Enable = RetData.U16Value;
        *MIN_Value = RetData.DoubleValue[0];
        *MAX_Value = RetData.DoubleValue[1];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveArrCmpPos(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Index, int32_t Position)
{
    int32_t ret;
    uint16_t SdoIndex;
    uint32_t AbortCode;
	if (Index > 199)
		return ECAT_ERR_INVALID_PARAM;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;
	SdoIndex = 0x2000 + (0x200 * EncNo) + Index;
    ret = ECAT_SetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x01, 4, Position, &AbortCode);
    return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveArrCmpPos(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Index, int32_t *Position)
{
	int32_t ret;
    uint16_t SdoIndex;
	uint32_t ObjectVal;
    uint32_t AbortCode;
	if (Index > 199)
		return ECAT_ERR_INVALID_PARAM;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;

	SdoIndex = 0x2000 + (0x200 * EncNo) + Index;
	ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x01, 4, &ObjectVal, &AbortCode);
    if(ret == 0)
    {
        *Position = ObjectVal;
    }
    return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveArrCmpEnable(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Index, uint8_t Enable)
{
    int32_t ret;
    uint16_t SdoIndex;
    uint32_t AbortCode;
	if (Index > 199)
		return ECAT_ERR_INVALID_PARAM;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;
    if(Enable)
        Enable = 1;
	SdoIndex = 0x2000 + (0x200 * EncNo) + Index;
    ret = ECAT_SetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x02, 1, Enable, &AbortCode);
    return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveArrCmpEnable(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Index, uint8_t *Enable)
{
	int32_t ret;
    uint16_t SdoIndex;
	uint32_t ObjectVal;
    uint32_t AbortCode;
	if (Index > 199)
		return ECAT_ERR_INVALID_PARAM;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;
	SdoIndex = 0x2000 + (0x200 * EncNo) + Index;
	ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x02, 1, &ObjectVal, &AbortCode);
    if(ret == 0)
    {
        *Enable = ObjectVal;
    }
    return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveArrCmpEndIdx(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint16_t EndIndex)
{
    int32_t ret;
    uint16_t SdoIndex;
    uint32_t AbortCode;
	if (EndIndex > 199)
		return ECAT_ERR_INVALID_PARAM;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;
	SdoIndex = 0x8001 + (0x10 * EncNo);
    ret = ECAT_SetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x0B, 2, EndIndex, &AbortCode);
    return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveArrCmpEndIdx(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint16_t *EndIndex)
{
	int32_t ret;
    uint16_t SdoIndex;
	uint32_t ObjectVal;
    uint32_t AbortCode;
	if (EncNo > 1)
		return ECAT_ERR_INVALID_PARAM;
	SdoIndex = 0x8001 + (0x10 * EncNo);
	ret = ECAT_GetSlaveSdoObject(DeviceNo, SlaveNo, SdoIndex, 0x0B, 2, &ObjectVal, &AbortCode);
    if(ret == 0)
    {
        *EndIndex = ObjectVal;
    }
    return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveArrCmpTrig(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_ARR_CMP_TRIG;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_SetSlaveSaveArrCmpData(uint16_t DeviceNo, uint16_t SlaveNo)
{
     int32_t ret;
    uint32_t SdoData;
 	uint32_t ObjectVal;
    uint8_t retry = 5;
    uint32_t AbortCode;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	//control code
    SdoData = 0x12345678;
    ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x01, 4, SdoData, 1000, &AbortCode);
    if(ret != 0)
        goto out_return;
    //set 0 
    SdoData = 0;
    ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x07, 1, SdoData, 1000, &AbortCode);
    if(ret != 0)
        goto out_return;
    //set 1 �}�l�s��
    SdoData = 1;
    ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x07, 1, SdoData, 1000, &AbortCode);
    if(ret == -1206)//�@�w�|time out
    {
        //�Y�b�o�̤@��Ūsdo�A�|��SDO�q�T���`�A�H�U3�ؤ�k�i�H��sdo�q�T��_���`
        //1.�_�}in port�����u�æA���s��
        //2.�Ҳ��_�q�í��W�q
        //3.�i�JOP�A���h�^PreOP
        usleep(10000);
    }
    else
        goto out_return;
    //�s�ieeprom��Ĥ@��sdo�R�O���w�X��,-1020
    ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x07, 1, &ObjectVal, 1000, &AbortCode);
    //�T�{SDO�q�T�L���`
    ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x07, 1, &ObjectVal, 1000, &AbortCode);
    if(ret != 0)
        goto out_return;

    //Load factory off
    SdoData = 0;
    ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x03, 1, SdoData, 1000, &AbortCode);
    if(ret != 0)
        goto out_return;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisDIActiveLevel(uint16_t DeviceNo, uint16_t AxisNo, uint8_t POT, uint8_t NOT, uint8_t ORG)
{
	int32_t ret;
	ecat_mc_axis_di_level_ops_t Ops;

	Ops.AxisNo = AxisNo;
    if(POT)
	    Ops.Value[0] = 1;
    else
	    Ops.Value[0] = 0;
    if(NOT)
	    Ops.Value[1] = 1;
    else
	    Ops.Value[1] = 0;
    if(ORG)
	    Ops.Value[2] = 1;
    else
	    Ops.Value[2] = 0;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_DI_ACTIVE_LEVEL,
		&Ops, sizeof(ecat_mc_axis_di_level_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDIActiveLevel(uint16_t DeviceNo, uint16_t AxisNo, uint8_t *POT, uint8_t *NOT, uint8_t *ORG)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_axis_di_level_ops_t RetData;

	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI_ACTIVE_LEVEL,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_axis_di_level_ops_t));
	if (ret == 0)
    {
		*POT = RetData.Value[0];
		*NOT = RetData.Value[1];
		*ORG = RetData.Value[2];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_60FD(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *DI)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI_60FD,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*DI = RetData.U32Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeDir_hontech(uint16_t DeviceNo, uint16_t AxisNo, int32_t HomeDir)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
    Ops.I32Data = HomeDir;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOME_DIR_HONTECH,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeDir_hontech(uint16_t DeviceNo, uint16_t AxisNo, int32_t *HomeDir)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_DIR_HONTECH,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
    {
		*HomeDir = RetData.I32Value;
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisLimitAsHome_Stepper(uint16_t DeviceNo, uint16_t AxisNo, int32_t Value)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
    Ops.I32Data = Value;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_LIMIT_AS_HOME_STEPPER,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisLimitAsHome_Stepper(uint16_t DeviceNo, uint16_t AxisNo, int32_t *Value)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_LIMIT_AS_HOME_STEPPER,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
    {
		*Value = RetData.I32Value;
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeLogic_hontech(uint16_t DeviceNo, uint16_t AxisNo, uint8_t NOT, uint8_t POT, uint8_t ORG)
{
	//ecat_mc_axis_di_level_ops_t Ops;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	uint16_t SlaveNo = EcatDevice[DeviceNo].McSlaveNo[AxisNo];
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo >= McAxisCnt)
		return ECAT_ERR_MC_INVALID_AXIS_NO;
	if (SlaveNo == 65535)//virtual axis
		return ECAT_ERR_FUNC_NOT_SUPPORT;
	int32_t ret;
	char Slavename[MAX_SLAVE_NAME_LENGTH];
	uint8_t AlState;
	uint16_t Alias;
	uint32_t ProductCode, VendorID, RevisionNo, SerialNo, SlaveType;
	ret = ECAT_GetSlaveInfo(DeviceNo, SlaveNo, &Alias, &ProductCode,
		&VendorID, &RevisionNo, &SerialNo, &AlState, &SlaveType, Slavename);
	if (ret < 0)
		return ret;

	uint32_t AbortCode = 0;
	uint32_t NOT_Value, POT_Value, ORG_Value;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (VendorID == 1647)//panasonic
	{
		//1: normal open
		//0: normal close
		if (NOT)
			NOT_Value = 0x020202;
		else
			NOT_Value = 0x828282;
		if (POT)
			POT_Value = 0x010101;
		else
			POT_Value = 0x818181;
		if (ORG)
			ORG_Value = 0x222222;
		else
			ORG_Value = 0xA2A2A2;
		//SI2 POT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3401, 0x00, 4, 0, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//SI3 NOT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3402, 0x00, 4, 0, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//SI4 HOME
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3403, 0x00, 4, 0, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//SI5 EXT1 /HOME
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3404, 0x00, 4, ORG_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//SI6 EXT2 /POT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3405, 0x00, 4, POT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//SI7 SI-MON3 /NOT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3406, 0x00, 4, NOT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
	}
	else if (VendorID == 4801360 && ((ProductCode == 2134355) || (ProductCode == 2135123)))
	{
		//1: normal open
		//0: normal close
		if (NOT)
			NOT_Value = 1;
		else
			NOT_Value = 0;
		if (POT)
			POT_Value = 1;
		else
			POT_Value = 0;

		uint16_t addIndex = EcatDevice[DeviceNo].McSubAxisNo[AxisNo] * 0x100;
		//NOT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x8012 + addIndex, 0x30, 1, NOT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		//POT
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x8012 + addIndex, 0x31, 1, POT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
	}
	else if (VendorID == 4801360 && ((ProductCode == 0x624F444D) || (ProductCode == 0x614F444D)))//ECAT-STEPTEC-ODM1 // ODM2
	{
		if (ORG)
			ORG_Value = 1;
		else
			ORG_Value = 0;
		uint16_t subIndex = EcatDevice[DeviceNo].McSubAxisNo[AxisNo] + 1;
		//ORG
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x2001, subIndex, 1, ORG_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
	}
	else
	{
		ret = ECAT_ERR_FUNC_NOT_SUPPORT;
	}

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeLogic_hontech(uint16_t DeviceNo, uint16_t AxisNo, uint8_t *NOT, uint8_t *POT, uint8_t *ORG)
{
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	uint16_t SlaveNo = EcatDevice[DeviceNo].McSlaveNo[AxisNo];
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo >= McAxisCnt)
		return ECAT_ERR_MC_INVALID_AXIS_NO;
	if (SlaveNo == 65535)//virtual axis
		return ECAT_ERR_FUNC_NOT_SUPPORT;
	int32_t ret;
	char Slavename[MAX_SLAVE_NAME_LENGTH];
	uint8_t AlState;
	uint16_t Alias;
	uint32_t ProductCode, VendorID, RevisionNo, SerialNo, SlaveType;
	ret = ECAT_GetSlaveInfo(DeviceNo, SlaveNo, &Alias, &ProductCode,
		&VendorID, &RevisionNo, &SerialNo, &AlState, &SlaveType, Slavename);
	if (ret < 0)
		return ret;

	uint32_t AbortCode = 0;
	uint32_t NOT_Value, POT_Value, ORG_Value;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (VendorID == 1647)//panasonic
	{
		//1: normal open
		//0: normal close

		//ORG
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3404, 0x00, 4, &ORG_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (ORG_Value == 0x222222)
			*ORG = 1;
		else if (ORG_Value == 0xA2A2A2)
			*ORG = 0;
		else
		{
			ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
			goto out_return;
		}
		//POT
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3405, 0x00, 4, &POT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (POT_Value == 0x010101)
			*POT = 1;
		else if (POT_Value == 0x818181)
			*POT = 0;
		else
		{
			ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
			goto out_return;
		}
		//NOT
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x3406, 0x00, 4, &NOT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (NOT_Value == 0x020202)
			*NOT = 1;
		else if (NOT_Value == 0x828282)
			*NOT = 0;
		else
		{
			ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
			goto out_return;
		}

	}
	else if (VendorID == 4801360 && ((ProductCode == 2134355) || (ProductCode == 2135123)))
	{
		//1: normal open
		//0: normal close
		uint16_t addIndex = EcatDevice[DeviceNo].McSubAxisNo[AxisNo] * 0x100;
		//NOT
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x8012 + addIndex, 0x30, 1, &NOT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (NOT_Value == 1)
			*NOT = 1;
		else
			*NOT = 0;

		//POT
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x8012 + addIndex, 0x31, 1, &POT_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (POT_Value == 1)
			*POT = 1;
		else
			*POT = 0;

		*ORG = -1;
	}
	else if (VendorID == 4801360 && ((ProductCode == 0x624F444D) || (ProductCode == 0x614F444D)))//ECAT-STEPTEC-ODM1 // ODM2
	{
		uint16_t subIndex = EcatDevice[DeviceNo].McSubAxisNo[AxisNo] + 1;
		//ORG
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x2001, subIndex, 1, &ORG_Value, 1000, &AbortCode);
		if (ret != 0)goto out_return;
		if (ORG_Value == 1)
			*ORG = 1;
		else
			*ORG = 0;
		*POT = -1;
		*NOT = -1;
	}
	else
	{
		ret = ECAT_ERR_FUNC_NOT_SUPPORT;
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SaveSlaveEEPROM(uint16_t DeviceNo, uint16_t SlaveNo)
{
    int32_t ret;
    char Slavename[MAX_SLAVE_NAME_LENGTH];
    uint8_t AlState;
    uint16_t Alias;
    uint32_t ProductCode, VendorID, RevisionNo, SerialNo, SlaveType; 
    ret = ECAT_GetSlaveInfo(DeviceNo, SlaveNo, &Alias, &ProductCode,
     &VendorID, &RevisionNo, &SerialNo, &AlState, &SlaveType, Slavename);
    if(ret < 0)
        return ret;
     uint32_t SdoData;
 	uint32_t ObjectVal;
    uint32_t AbortCode = 0;
    ret = ECAT_ERR_FUNC_NOT_SUPPORT;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
    if(VendorID == 4801360 && ((ProductCode == 2134355) || (ProductCode == 2135123)))//ECAT-2094S/ 2091S
    {
       //control code
        SdoData = 0x12345678;
        ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x01, 4, SdoData, 1000, &AbortCode);
        if(ret != 0)
            goto out_return;
        //set 0 
        SdoData = 0;
        ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x04, 1, SdoData, 1000, &AbortCode);
        if(ret != 0)
            goto out_return;
        //set 1 �}�l�s��
        SdoData = 1;
        ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x04, 1, SdoData, 1000, &AbortCode);
        if(ret == -1206)//�@�w�|time out
        {
            //�Y�b�o�̤@��Ūsdo�A�|��SDO�q�T���`�A�H�U3�ؤ�k�i�H��sdo�q�T��_���`
            //1.�_�}in port�����u�æA���s��
            //2.�Ҳ��_�q�í��W�q
            //3.�i�JOP�A���h�^PreOP
            usleep(2000000);
        }
        else if(ret != 0)
            goto out_return;
        //�s�ieeprom��Ĥ@��sdo�R�O���w�X��,-1020
        ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x04, 1, &ObjectVal, 1000, &AbortCode);
        //�T�{SDO�q�T�L���`
        ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x04, 1, &ObjectVal, 1000, &AbortCode);
        if(ret != 0)
            goto out_return;
		//check error encountered falg
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x05, 1, &ObjectVal, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		if (ObjectVal != 0)
		{
			ret = ECAT_ERR_SLAVE_EEPROM_ERROR;
			goto out_return;
		}
        //Load factory off
        SdoData = 0;
        ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x03, 1, SdoData, 1000, &AbortCode);
        if(ret != 0)
            goto out_return;
		//check error encountered falg
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0xF008, 0x05, 1, &ObjectVal, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		if (ObjectVal != 0)
		{
			ret = ECAT_ERR_SLAVE_EEPROM_ERROR;
			goto out_return;
		}
    }
	else if (VendorID == 1647)//panasonic
	{
		//�M����e���~
		SdoData = 0x80;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x6040, 0x00, 2, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		//set 0x65766173 �}�l�s��
		SdoData = 0x65766173;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x1010, 0x01, 4, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
	}
	else if (VendorID == 4801360 && ((ProductCode == 0x624F444D) || (ProductCode == 0x614F444D)))//ECAT-STEPTEC-ODM1 // ODM2
	{
		//�M����e���~
		SdoData = 0x80;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x6040, 0x00, 2, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		//set Load Factory 0
		SdoData = 0;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x5000, 0x02, 4, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		//set 0 �ǳƦs��
		SdoData = 1;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x5000, 0x01, 4, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		//set 1 �}�l�s��
		SdoData = 1;
		ret = _SetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x5000, 0x01, 4, SdoData, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		//check error
		ret = _GetSlaveSdoObjectEx(DeviceNo, SlaveNo, 0x5001, 0x01, 4, &ObjectVal, 1000, &AbortCode);
		if (ret != 0)
			goto out_return;
		if (ObjectVal != 0)
		{
			ret = ECAT_ERR_SLAVE_EEPROM_ERROR;
			goto out_return;
		}
	}

out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
    if(ret != 0 && AbortCode == 0x08000022)
    {
        ret = ECAT_ERR_OPTASK;
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisActualPosition(uint16_t DeviceNo, uint16_t AxisNo, double Position)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Position;

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_SET_AXIS_ACTUAL_POSITION,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisCommandPosition(uint16_t DeviceNo, uint16_t AxisNo, double Position)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Position;

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_SET_AXIS_COMMAND_POSITION,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_hontech(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *DI)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	int i;
	Ops.No = AxisNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI_HONTECH,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0)
		if (AxisNo == 65535)
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
			{
				*(DI + i) = RetData.U32Value32[i];
			}
		else
			*DI = RetData.U32Value32[0];
	return ret;

}
/*****************************************************************************/
int32_t ECAT_McSetAxisHomeExtDist(uint16_t DeviceNo, uint16_t AxisNo, double Value)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Value;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_HOMEEXTDIST,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeExtDist(uint16_t DeviceNo, uint16_t AxisNo, double *Value)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOMEEXTDIST,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Value = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_OpenMotionConfigEx(uint16_t DeviceNo, char* bstrFileName, uint16_t *AxisCnt, uint16_t SlaveNo[],
                              uint16_t SubAxisNo[], double PPU[], int32_t HomeMethod[], double HomeSpeedSeachSw[],
                              double HomeSpeedSeachZr[], double HomeAcc[], uint32_t EncoderPPR[], uint32_t MotorPPR[])
{
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
    int Cnt = 0;
    int ret = ECAT_ERR_READ_FILE;
	ezxml_t f1 = ezxml_parse_file(bstrFileName), Axis, elem;
	if(f1 == NULL)
		goto ret_return;
	if (strcmp(f1->name, "MotionConfig") == 0)
	{
		for (Axis = ezxml_child(f1, "Axis"); Axis; Axis = Axis->next) {
			if (Cnt >= MC_AXIS_NO_MAX)
			{
				ret = ECAT_ERR_MC_AXIS_CNT_EXCEEDED;
				goto ret_return;
			}
			if((elem = ezxml_child(Axis, "SlaveNo")) != NULL)
				SlaveNo[Cnt] = atoi(elem->txt);
			if((elem = ezxml_child(Axis, "SubAxisNo")) != NULL)
				SubAxisNo[Cnt] = atoi(elem->txt);
			if((elem = ezxml_child(Axis, "PPU")) != NULL)
				PPU[Cnt] = atof(elem->txt);
			if((elem = ezxml_child(Axis, "HomeMethod")) != NULL)
				HomeMethod[Cnt] = atoi(elem->txt);
			if((elem = ezxml_child(Axis, "HomeSpeedSW")) != NULL)
				HomeSpeedSeachSw[Cnt] = atof(elem->txt);
			if((elem = ezxml_child(Axis, "HomeSpeedZR")) != NULL)
				HomeSpeedSeachZr[Cnt] = atof(elem->txt);
			if((elem = ezxml_child(Axis, "HomeAcc")) != NULL)
				HomeAcc[Cnt] = atof(elem->txt);
			if((elem = ezxml_child(Axis, "EncoderPPR")) != NULL)
				EncoderPPR[Cnt] = atoi(elem->txt);
			if((elem = ezxml_child(Axis, "MotorPPR")) != NULL)
				MotorPPR[Cnt] = atoi(elem->txt);
			Cnt++;
		}
	}
    ret = 0;
ret_return:
	ezxml_free(f1);
    *AxisCnt = Cnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
    return ret;
}
/*****************************************************************************/
int32_t	ECAT_StGet_axis_Fwd_model_J(uint16_t DeviceNo, uint16_t AxisNo, float *J)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops ;
	ecat_mc_stick_return_t RetData;
	int i;
	Ops.AxisNo = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_GET_AXIS_FWD_MODEL_J,
		&Ops, sizeof(ecat_mc_stick_ops_t), &RetData, sizeof(ecat_mc_stick_return_t));
    if (ret == 0)
    {
	    if (AxisNo == 65535)
		    for (i = 0; i < MAX_MODEL_NO; i++)
		    {
			    *(J + i) = RetData.FloatValue_Multi[i];
		    }
	    else
		    *J = RetData.FloatValue_Multi[0];
    }
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_StSet_axis_max_Jerk(uint16_t DeviceNo, uint16_t AxisNo, float J)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops ;
	Ops.AxisNo = AxisNo;
    Ops.floatData[0] = J;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_AXIS_MAX_JERK,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccDecTime_Stepper(uint16_t DeviceNo, uint16_t AxisNo, uint16_t AccTime_ms, uint16_t DecTime_ms)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = AccTime_ms;
	Ops.U16Data[1] = DecTime_ms;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ACC_DEC_TIME_STEPPER,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccDecTime_Stepper(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *AccTime_ms, uint16_t *DecTime_ms)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACC_DEC_TIME_STEPPER,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_t));
	if (ret == 0)
    {
		*AccTime_ms = RetData.U16Value[0];
		*DecTime_ms = RetData.U16Value[1];
    }
	return ret;
}
/*****************************************************************************/
int32_t	ECAT_STSet_grp_link(uint16_t DeviceNo, uint16_t GrpNo, uint16_t m_AxisNo, uint16_t s_AxisNo)
{
	int32_t ret;
	ecat_mc_stick_ops_t Ops ;
	Ops.U16Data[0] = GrpNo;
	Ops.U16Data[1] = m_AxisNo;
	Ops.U16Data[2] = s_AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_ST_SET_GRP_LINK,
		&Ops, sizeof(ecat_mc_stick_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceStateEx(uint16_t DeviceNo, uint32_t *LinkUp, uint32_t *SlavesResp, uint32_t *AlStates, uint32_t *Wc)
{
	int32_t ret;
	ecat_device_state_ex_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_DEVICE_STATE_EX,
		NULL, 0, &RetData, sizeof(ecat_device_state_ex_t));
	if (ret == 0)
	{
		*LinkUp = RetData.LinkUp;
		*SlavesResp = RetData.SlavesResp;
		*AlStates = RetData.AlStates;
		*Wc = RetData.Wc;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_UploadEc_Generic(uint16_t DeviceNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	int32_t ret;
	ecat_write_file_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	ecat_write_file_retrun_t RetData;
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_EC_GENERIC, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_UploadEc_Master(uint16_t DeviceNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	int32_t ret;
	ecat_write_file_ops_t Ops;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	ecat_write_file_retrun_t RetData;
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_EC_MASTER, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockRegErrReadRequest(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Type)
{
	int32_t ret;
	ecat_slave_non_reg_ops_t Ops;

	Ops.No = SlaveNo;
	Ops.Type = Type;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_ERR_READ_REQ,
		&Ops, sizeof(ecat_slave_non_reg_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockRegErrReadState(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Type, uint8_t *Value1, uint8_t *Value2)
{
	int32_t ret;
	ecat_slave_non_reg_ops_t Ops;
	ecat_slave_non_reg_return_t RetData;
    uint8_t Value[8];
    Ops.No = SlaveNo;
	Ops.Type = Type;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_ERR_READ_STATE,
		&Ops, sizeof(ecat_slave_non_reg_ops_t), &RetData, sizeof(ecat_slave_non_reg_return_t));
	if (ret == 0)
    {
        memcpy(Value, RetData.Val, sizeof(Value));

		Value1[0] = Value[0];
		Value1[1] = Value[1];
		Value1[2] = Value[2];
		Value1[3] = Value[3];

		Value2[0] = Value[4];
		Value2[1] = Value[5];
		Value2[2] = Value[6];
		Value2[3] = Value[7];
    }

	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockRegErClrRequest(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Type)
{
	int32_t ret;
	ecat_slave_non_reg_ops_t Ops;

	Ops.No = SlaveNo;
	Ops.Type = Type;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_ERR_CLR_REQ,
		&Ops, sizeof(ecat_slave_non_reg_ops_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_SlaveNonBlockRegErrClrState(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Type)
{
	int32_t ret;
	ecat_slave_non_reg_ops_t Ops;
    Ops.No = SlaveNo;
	Ops.Type = Type;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_NONBLOCK_ERR_CLR_STATE,
		&Ops, sizeof(ecat_slave_non_reg_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_DownloadSyslog(uint16_t DeviceNo, uint16_t LogNo, uint32_t *Offset, char *Data, uint32_t *DataSize, uint8_t *LastFlag)
{
	int32_t ret;
	ecat_read_file_ops_t Ops;
	ecat_read_file_return_t RetData;
	uint32_t OpsSize = sizeof(ecat_read_file_ops_t);
	uint32_t RetSize = sizeof(ecat_read_file_return_t);
	uint32_t DataOffset = 0;

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)

	Ops.No = LogNo;
	if (*LastFlag == 255)
	{
		Ops.GetFileSize = 1;
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_DOWNLOAD_SYSLOG, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			*DataSize = RetData.Size;
			ret = 0;
			goto out_return;
		}
	}
	else
		Ops.GetFileSize = 0;

	while (1)
	{
		if ((ret = SetCommandData(&EcatDevice[DeviceNo], ECAT_IPC_CMD_DOWNLOAD_SYSLOG, (uint32_t*)&Ops, OpsSize)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			memcpy((Data + (DataOffset)), RetData.Data, RetData.Size);
			DataOffset += RetData.Size;
			if ((RetData.LastFlag) || (*DataSize < (DataOffset + ECAT_IPC_DATA_SIZE_MAX)))
			{
				*DataSize = DataOffset;
				*Offset = RetData.Offset;
				*LastFlag = RetData.LastFlag;
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveRegErr(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t Type, uint8_t *Value1, uint8_t *Value2)
{
    return 0;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomePos_hontech(uint16_t DeviceNo, uint16_t AxisNo, double *Pos)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOMEPOS_HONTECH,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		*Pos = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveEncIdxLatchBufferEnable(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t Enable)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_SET_LATCH_BUFFER_ENABLE;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	Ops.Arg[0] = Enable;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t ECAT_GetSlaveEncIdxLatchBufferEnable(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, uint8_t *Enable)
{
	int32_t ret;
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_GET_LATCH_BUFFER_ENABLE;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	ret = CommonSlaveEnc(DeviceNo, &Ops);
	if (ret == 0)
	{
		*Enable = Ops.Arg[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_ResetSlaveEncIdxLatchBuffer(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo)
{
	ecat_slave_enc_data_t Ops;
	Ops.RunMode = SLAVE_ENC_RM_CLEAR_LATCH_BUFFER;
	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
	return CommonSlaveEnc(DeviceNo, &Ops);
}
/*****************************************************************************/
int32_t	ECAT_GetSlaveEncIdxLatchBuffer(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t EncNo, int32_t GetCount, int32_t *ActualGetCount, int32_t *Data)
{
    int32_t ret;
    ecat_slave_enc_data_t Ops;
    ecat_enc_rec_return_t RetData;
   	Ops.SlaveNo = SlaveNo;
	Ops.EncNo = EncNo;
    Ops.I32Value[0] = GetCount;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_ENC_LATCH_BUFFER,
		&Ops, sizeof(ecat_slave_enc_data_t), &RetData, sizeof(ecat_enc_rec_return_t));
    if (ret == 0)
    {
        *ActualGetCount = RetData.ActualGetCount;
        memcpy(Data, RetData.Data, sizeof(int32_t) * RetData.ActualGetCount);
    }   
    return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccUnit_Stepper(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Unit, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	ecat_slave_sdo_return_t RetData;

	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Unit;

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_SET_AXIS_ACC_UNIT_STEPPER,
		&Ops, sizeof(ecat_mc_axis_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret != 0)
		*AbortCode = RetData.AbortCode;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccUnit_Stepper(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Unit, uint32_t *AbortCode)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_slave_sdo_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_MC_GET_AXIS_ACC_UNIT_STEPPER,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_slave_sdo_return_t));
	if (ret == 0)
		*Unit = RetData.ObjectVal;
	else
		*AbortCode = RetData.AbortCode;

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisVelAccScale(uint16_t DeviceNo, uint16_t AxisNo, double VelScal, double AccScal)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = VelScal;
	Ops.DoubleData[1] = AccScal;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_VEL_ACC_SCALE,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisVelAccScale(uint16_t DeviceNo, uint16_t AxisNo, double *VelScal, double *AccScal)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_VEL_ACC_SCALE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
    {
        *VelScal = RetData.DoubleValue[0];
        *AccScal = RetData.DoubleValue[1];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisInpSignal(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Offset, uint16_t Bit, int32_t Enable)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Offset;
	Ops.U16Data[1] = Bit;
    Ops.I32Data = Enable;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_INP_SIGNAL,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisInpSignal(uint16_t DeviceNo, uint16_t AxisNo, uint16_t *Offset, uint16_t *Bit, int32_t *Enable)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_INP_SIGNAL,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_t));
	if (ret == 0)
    {
        *Offset = RetData.U16Value[0];
        *Bit = RetData.U16Value[1];
        *Enable = RetData.I32Value[0];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_Upload_Ixecat(uint16_t DeviceNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;


	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_IXECAT, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_Upload_Ixpciecat(uint16_t DeviceNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;


	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		if ((ret = SetCommandDataEx(&EcatDevice[DeviceNo], ECAT_IPC_CMD_UPLOAD_IXPCIECAT, (uint32_t*)&Ops, OpsSize, 2000)) < 0)
			goto out_return;

		if ((ret = GetReturnData(&EcatDevice[DeviceNo], (uint32_t*)&RetData, RetSize)) < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisInpCompare(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Time_ms, double PosWindow, int32_t Enable)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U32Data = Time_ms;
	Ops.DoubleData[0] = PosWindow;
    Ops.I32Data = Enable;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_INP_COMPARE,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisInpCompare(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *Time_ms, double *PosWindow, int32_t *Enable)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_INP_COMPARE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_t));
	if (ret == 0)
    {
        *Time_ms = RetData.U32Value[0];
        *PosWindow = RetData.DoubleValue[0];
        *Enable = RetData.I32Value[0];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_WriteSpec(uint16_t DeviceNo, uint8_t* Data)
{
	int32_t ret;
	ecat_write_max_axis_cnt_arg_t Ops;

	memcpy(Ops.Data, Data, 128);

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_WRITE_SPEC,
		&Ops, sizeof(ecat_write_max_axis_cnt_arg_t), NULL, 0);

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisInpTimeOut(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Time_ms)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U32Data = Time_ms;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_INP_TIMEOUT,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisInpTimeOut(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *Time_ms)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_INP_TIMEOUT,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_t));
	if (ret == 0)
    {
        *Time_ms = RetData.U32Value[0];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisWanErrEnable(uint16_t DeviceNo, uint16_t AxisNo, uint32_t Enable)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U32Data = Enable;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_WAN_ERR,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisWanErrEnable(uint16_t DeviceNo, uint16_t AxisNo, uint32_t *Enable)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_WAN_ERR,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_t));
	if (ret == 0)
    {
        *Enable = RetData.U32Value[0];
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetIxecatVir(uint16_t DeviceNo, uint16_t No)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;

	Ops.No = No;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_IXECAT_VIR,
		&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetIxecatVir(uint16_t DeviceNo, uint16_t *No)
{
	int32_t ret;
	ecat_ipc_base_ops_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_IXECAT_VIR,
		NULL, 0, &RetData, sizeof(ecat_ipc_base_ops_t));
	if (ret == 0)
    {
        *No = RetData.No;
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetMultiSlaveDO(uint16_t DeviceNo, uint16_t SlaveNo[], uint32_t Value[], uint16_t SlaveCnt)
{
	int32_t ret;
	uint16_t i;
	ecat_multi_slave_do_data_t Ops;
	if (SlaveCnt > MULTI_SLAVE_DO_MAX) return ECAT_ERR_SLAVE_CNT_EXCEEDED;

	memcpy(Ops.SlaveNo, SlaveNo, sizeof(uint16_t) * SlaveCnt);
	memcpy(Ops.U32Value, Value, sizeof(uint32_t) * SlaveCnt);
	Ops.SlaveCnt = SlaveCnt;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_MULTI_SLAVE_DO,
		&Ops, sizeof(ecat_multi_slave_do_data_t), NULL, 0);
   	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetMultiSlaveDO(uint16_t DeviceNo, uint16_t SlaveNo[], uint32_t *Value, uint16_t SlaveCnt)
{
	int32_t ret;
	uint16_t i;
	ecat_multi_slave_do_data_t Ops;
	ecat_multi_slave_do_data_t RetData;

	if (SlaveCnt > MULTI_SLAVE_DO_MAX) return ECAT_ERR_SLAVE_CNT_EXCEEDED;

	memcpy(Ops.SlaveNo, SlaveNo, sizeof(uint16_t) * SlaveCnt);
	Ops.SlaveCnt = SlaveCnt;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_MULTI_SLAVE_DO,
		&Ops, sizeof(ecat_multi_slave_do_data_t), &RetData, sizeof(ecat_multi_slave_do_data_t));
    if(ret == 0)
    {
		for (i = 0; i < SlaveCnt; i++)
		{
			*(Value + i) = RetData.U32Value[i];
		}
    }
   	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisDefaultMode(uint16_t DeviceNo, uint16_t AxisNo, int8_t Mode)
{
	int32_t ret;
	ecat_mc_axis_general_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.I8Value[0] = Mode;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_DEFAULTMODE,
		&Ops, sizeof(ecat_mc_axis_general_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDefaultMode(uint16_t DeviceNo, uint16_t AxisNo, int8_t *Mode)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_general_return_ex_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DEFAULTMODE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_general_return_ex_t));
	if (ret == 0)
		*Mode = RetData.I8Value[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceIgnoreWC(uint16_t DeviceNo, int8_t Enable)
{
	int32_t ret;
	ecat_general_return_ex_t Ops;

	Ops.I8Value[0] = Enable;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_IGNORE_WC,
		&Ops, sizeof(ecat_general_return_ex_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceIgnoreWC(uint16_t DeviceNo, int8_t *Enable)
{
	int32_t ret;
	ecat_general_return_ex_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_IGNORE_WC,
		NULL, 0, &RetData, sizeof(ecat_general_return_ex_t));
	if (ret == 0)
		*Enable = RetData.I8Value[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetIghVir(uint16_t DeviceNo, uint16_t No)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;

	Ops.No = No;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_IGH_VIR,
		&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetIghVir(uint16_t DeviceNo, uint16_t *No)
{
	int32_t ret;
	ecat_ipc_base_ops_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_IGH_VIR,
		NULL, 0, &RetData, sizeof(ecat_ipc_base_ops_t));
	if (ret == 0)
    {
        *No = RetData.No;
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McInit_Ex(uint16_t DeviceNo, char* FileName, uint16_t SlaveNo[], uint16_t SubAxisNo[], uint16_t* AxisCnt)
{
	CHECK_ECAT_DEVICE_NO(DeviceNo)
	CHECK_ECAT_DEVICE_IS_OPEN(EcatDevice[DeviceNo].IsOpen)
    int32_t HomeMethod[MC_AXIS_NO_MAX] = {0};
    double PPU[MC_AXIS_NO_MAX] = {0}, HomeSpeedSeachSw[MC_AXIS_NO_MAX] = {0}, HomeSpeedSeachZr[MC_AXIS_NO_MAX] = {0}, HomeAcc[MC_AXIS_NO_MAX] = {0};
	uint32_t EncoderPPR[MC_AXIS_NO_MAX] = { 0 }, MotorPPR[MC_AXIS_NO_MAX] = { 0 };
	uint16_t AccTime[MC_AXIS_NO_MAX] = { 0 };
	uint8_t DiLev[MC_AXIS_NO_MAX];
    uint16_t Cnt = 0;
    int ret = ECAT_ERR_READ_FILE;
	ezxml_t f1 = ezxml_parse_file(FileName), Axis, elem;
	if(f1 == NULL)
		goto ret_return;
	if (strcmp(f1->name, "MotionConfig") == 0)
	{
		for (Axis = ezxml_child(f1, "Axis"); Axis; Axis = Axis->next) {
			if (Cnt >= MC_AXIS_NO_MAX)
			{
				ret = ECAT_ERR_MC_AXIS_CNT_EXCEEDED;
				goto ret_return;
			}
			if((elem = ezxml_child(Axis, "SlaveNo")) != NULL)
				SlaveNo[Cnt] = atoi(elem->txt);
			if((elem = ezxml_child(Axis, "SubAxisNo")) != NULL)
				SubAxisNo[Cnt] = atoi(elem->txt);
			Cnt++;
		}
	}
    else
        goto ret_return;
	ret = ECAT_McInit(DeviceNo, SlaveNo, SubAxisNo, Cnt);
    if(ret != 0)
        goto ret_return;
    *AxisCnt = Cnt;
    uint16_t AxisNo = 0;

	for (Axis = ezxml_child(f1, "Axis"); Axis; Axis = Axis->next) {
		if((elem = ezxml_child(Axis, "PPU")) != NULL)
		{
			PPU[AxisNo] = atof(elem->txt);
            if((ret = ECAT_McSetAxisPPU(DeviceNo, AxisNo, PPU[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "HomeMethod")) != NULL)
		{
			HomeMethod[AxisNo] = atoi(elem->txt);
            if((ret = ECAT_McSetAxisHomeMethod(DeviceNo, AxisNo, HomeMethod[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "HomeSpeedSW")) != NULL)
		{
			HomeSpeedSeachSw[AxisNo] = atof(elem->txt);
            if((ret = ECAT_McSetAxisHomeSpeed(DeviceNo,AxisNo, HomeSpeedSeachSw[AxisNo], HomeSpeedSeachZr[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "HomeSpeedZR")) != NULL)
		{
			HomeSpeedSeachZr[AxisNo] = atof(elem->txt);
            if((ret = ECAT_McSetAxisHomeSpeed(DeviceNo,AxisNo, HomeSpeedSeachSw[AxisNo], HomeSpeedSeachZr[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "HomeAcc")) != NULL)
		{
			HomeAcc[AxisNo] = atof(elem->txt);
            if((ret = ECAT_McSetAxisHomeAcc(DeviceNo,AxisNo, HomeAcc[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "EncoderPPR")) != NULL)
		{
			EncoderPPR[AxisNo] = atoi(elem->txt);
            if((ret = ECAT_McSetAxisEncoderPPR(DeviceNo,AxisNo, EncoderPPR[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "MotorPPR")) != NULL)
		{
			MotorPPR[AxisNo] = atoi(elem->txt);
            if((ret = ECAT_McSetAxisMotorPPR(DeviceNo, AxisNo,  MotorPPR[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "DiLev")) != NULL)
		{
			DiLev[AxisNo] = atoi(elem->txt);
			if ((ret = ECAT_McSetAxisDIActiveLevel(DeviceNo, AxisNo, DiLev[AxisNo], DiLev[AxisNo], DiLev[AxisNo])) != 0)
                goto ret_return;
		}
		if((elem = ezxml_child(Axis, "AccTime")) != NULL)
		{
			AccTime[AxisNo] = atoi(elem->txt);
			if ((ret = ECAT_McSetAxisAccTime(DeviceNo, AxisNo, AccTime[AxisNo])) != 0)
                goto ret_return;
		}
		AxisNo++;
	}
ret_return:
	ezxml_free(f1);
    return ret;
}
/*****************************************************************************/
int32_t ECAT_UploadFOE(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t Offset, char *Data, uint32_t DataSize, uint8_t LastFlag)
{
	int32_t ret;
	ecat_write_file_ops_t Ops;
	ecat_write_file_retrun_t RetData;
	uint32_t OpsSize = sizeof(ecat_write_file_ops_t);
	uint32_t RetSize = sizeof(ecat_write_file_retrun_t);
	uint32_t DataOffset = 0;


	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if(LastFlag == 2)
    {
        Ops.No = SlaveNo;
        memcpy(Ops.Data, Data, DataSize);
        Ops.Size = DataSize;
        Ops.LastFlag = 2;

		ret = GenericSendCommand_ex(DeviceNo, 0, ECAT_IPC_CMD_UPLOAD_FOE,
			&Ops, sizeof(ecat_write_file_ops_t), NULL, 0);
		if(ret < 0)
			goto out_return;
	}
	while (1)
	{
		if (DataSize > ECAT_IPC_DATA_SIZE_MAX)
		{
			Ops.Size = ECAT_IPC_DATA_SIZE_MAX;
			Ops.Offset = Offset + DataOffset;
			Ops.LastFlag = 0;
		}
		else
		{
			Ops.Size = DataSize;
			Ops.Offset = Offset + DataOffset;
			if (LastFlag)
				Ops.LastFlag = 1;
			else
				Ops.LastFlag = 0;
		}

		memcpy(Ops.Data, (Data + (DataOffset)), Ops.Size);

		ret = GenericSendCommand_ex(DeviceNo, 0, ECAT_IPC_CMD_UPLOAD_FOE,
			&Ops, sizeof(ecat_write_file_ops_t), &RetData, sizeof(ecat_write_file_retrun_t));
		if (ret < 0)
			goto out_return;
		else
		{
			DataOffset += RetData.WriteBytes;
			DataSize -= RetData.WriteBytes;
			if (DataSize == 0)
			{
				ret = 0;
				goto out_return;
			}
		}
	}
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetMultiSlaveDO_AutoOff(uint16_t DeviceNo, uint16_t SlaveNo[], uint32_t Value[], uint32_t Width[], uint32_t Mask[], uint16_t SlaveCnt)
{
	int32_t ret;
	uint16_t i;
	ecat_multi_slave_do_data_t Ops;
	if (SlaveCnt > MULTI_SLAVE_DO_MAX) return ECAT_ERR_SLAVE_CNT_EXCEEDED;

	memcpy(Ops.SlaveNo, SlaveNo, sizeof(uint16_t) * SlaveCnt);
	memcpy(Ops.U32Value, Value, sizeof(uint32_t) * SlaveCnt);
	memcpy(Ops.Width, Width, sizeof(uint32_t) * SlaveCnt);
	memcpy(Ops.mask, Mask, sizeof(uint32_t) * SlaveCnt);
	Ops.SlaveCnt = SlaveCnt;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_MULTI_SLAVE_DO_AUTO_OFF,
		&Ops, sizeof(ecat_multi_slave_do_data_t), NULL, 0);
   	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetGroupAxisState(uint16_t DeviceNo, uint16_t GroupNo, uint16_t *AxisNo, uint32_t *State, uint16_t *AxisCount)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_tt RetData;
	int i;

   	Ops.No = GroupNo;
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_GROUP_AXIS_STATE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_tt));
	if (ret == 0)
    {
        *AxisCount = RetData.U16Value;
	    for (i = 0; i < *AxisCount; i++)
	    {
            if(AxisNo)
		        *(AxisNo + i) = RetData.U16Value32[i];
            if(State)
		        *(State + i) = RetData.U32Value32[i];
	    }
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisHomeOffset_hontech(uint16_t DeviceNo, uint16_t AxisNo, double *Pos)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_double_multi_t RetData;
	int i;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_HOME_OFFSET_HONTECH,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_double_multi_t));
	if (ret == 0)
		if (AxisNo == 65535)
			for (i = 0; i < MC_AXIS_NO_MAX; i++)
			{
				*(Pos + i) = RetData.DoubleValue_multi[i];
			}
		else
			*Pos = RetData.DoubleValue_multi[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccDecUnit(uint16_t DeviceNo, uint16_t AxisNo, uint16_t Unit)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = Unit;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ACC_DEC_UNIT,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccDecUnit(uint16_t DeviceNo, uint16_t AxisNo, uint16_t* Unit)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACC_DEC_UNIT,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		* Unit = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisAccDecRate(uint16_t DeviceNo, uint16_t AxisNo, double Rate)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Rate;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_ACC_DEC_RATE,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisAccDecRate(uint16_t DeviceNo, uint16_t AxisNo, double* Rate)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACC_DEC_RATE,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		* Rate = RetData.DoubleValue[0];
		return ret;
}
/*****************************************************************************/
int32_t ECAT_SetStepperCompact(uint16_t DeviceNo, uint16_t Enable)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;
	Ops.U16Data[0] = Enable;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SET_STEPPER_COMPACT,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetStepperCompact(uint16_t DeviceNo, uint16_t * Enable)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_STEPPER_COMPACT,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		* Enable = RetData.U16Value;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisStartVelocity(uint16_t DeviceNo, uint16_t AxisNo, double Vel)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Vel;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_START_VELOCITY,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisStartVelocity(uint16_t DeviceNo, uint16_t AxisNo, double* Vel)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_t RetData;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_START_VELOCITY,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_t));
	if (ret == 0)
		* Vel = RetData.DoubleValue[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisState_Ex(uint16_t DeviceNo, uint16_t AxisNo, uint32_t* State)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_u32_t RetData;
	int i;
	uint32_t RetSize;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)

	if(AxisNo == 65535)
		RetSize = sizeof(uint32_t) * McAxisCnt;
	else
		RetSize = sizeof(uint32_t);
	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_STATE_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(State + i) = RetData.U32Value[i];
			}
		else
			*State = RetData.U32Value[0];

	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisLastError_Ex(uint16_t DeviceNo, uint16_t AxisNo, int32_t * Error)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(int32_t) * McAxisCnt;
	else
		RetSize = sizeof(int32_t);
	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_LAST_ERROR_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(Error + i) = RetData.I32Value[i];
			}
		else
			*Error = RetData.I32Value[0];
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDriveError_Ex(uint16_t DeviceNo, uint16_t AxisNo, uint16_t * Error)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_u16_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(uint16_t) * McAxisCnt;
	else
		RetSize = sizeof(uint16_t);
	RetSize += RetSize % ECAT_IPC_SHM_ALIGN;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DRIVE_ERROR_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(Error + i) = RetData.U16Value[i];
			}
		else
			*Error = RetData.U16Value[0];
	return ret;
}
///*****************************************************************************/
int32_t ECAT_McGetAxisActualPos_Ex(uint16_t DeviceNo, uint16_t AxisNo, double* Pos)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(int32_t) * McAxisCnt;
	else
		RetSize = sizeof(int32_t);

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACT_POS_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(Pos + i) = RetData.I32Value[i] / EcatDevice[DeviceNo].AxisPPU[i];
			}
		else
			*Pos = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[AxisNo];
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisCommandPos_Ex(uint16_t DeviceNo, uint16_t AxisNo, double* Pos)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(int32_t) * McAxisCnt;
	else
		RetSize = sizeof(int32_t);

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_CMD_POS_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (AxisNo == 65535)
			for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
			{
				*(Pos + i) = RetData.I32Value[i] / EcatDevice[DeviceNo].AxisPPU[i];
			}
		else
			*Pos = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[AxisNo];
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualVel_Ex(uint16_t DeviceNo, uint16_t AxisNo, double* Vel)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(int32_t) * McAxisCnt;
	else
		RetSize = sizeof(int32_t);

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACT_VEL_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (AxisNo == 65535)
			for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
			{
				*(Vel + i) = RetData.I32Value[i] / EcatDevice[DeviceNo].AxisPPU[i];
			}
		else
			*Vel = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[AxisNo];
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_Ex(uint16_t DeviceNo, uint16_t AxisNo, uint32_t * DI)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_u32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(uint32_t) * McAxisCnt;
	else
		RetSize = sizeof(uint32_t);
		Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(RetSize));
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(DI + i) = RetData.U32Value[i];
			}
		else
			*DI = RetData.U32Value[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualTorque_Ex(uint16_t DeviceNo, uint16_t AxisNo, double* Torque)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i16_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(int16_t) * McAxisCnt;
	else
		RetSize = sizeof(int16_t);
	RetSize += RetSize % ECAT_IPC_SHM_ALIGN;
	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_ACTUAL_TORQUE_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(Torque + i) = RetData.I16Value[i];
			}
		else
			*Torque = RetData.I16Value[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_hontech_Ex(uint16_t DeviceNo, uint16_t AxisNo, uint32_t * DI)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_u32_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (AxisNo == 65535)
		RetSize = sizeof(uint32_t) * McAxisCnt;
	else
		RetSize = sizeof(uint32_t);
	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_DI_HONTECH_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		if (AxisNo == 65535)
			for (i = 0; i < McAxisCnt; i++)
			{
				*(DI + i) = RetData.U32Value[i];
			}
		else
			*DI = RetData.U32Value[0];
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisPos_Ex(uint16_t DeviceNo, double* ActPos, double* CmdPos)
{

	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_i32_2_t RetData;
	int i;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_GET_AXIS_POS_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, sizeof(ecat_mc_return_i32_2_t));
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
		{
			EcatDevice[DeviceNo].ActualPos[i] = RetData.I3232Value[i].I32Value1 / EcatDevice[DeviceNo].AxisPPU[i];
			EcatDevice[DeviceNo].CommandPos[i] = RetData.I3232Value[i].I32Value2 / EcatDevice[DeviceNo].AxisPPU[i];
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisPos_Ex_TimeOut(uint16_t DeviceNo, uint64_t TimeOut)//ms
{
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	EcatDevice[DeviceNo].PosQuadTimeOut = TimeOut;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return 0;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisActualPos_Directly(uint16_t DeviceNo, uint16_t AxisNo, double* Pos)
{
	int32_t ret = 0;
	ecat_mc_return_i32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DirectlyEnable = EcatDevice[DeviceNo].DirectlyEnable;
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DirectlyEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
    
	if (AxisNo < McAxisCnt)
		ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x00+AxisNo), (uint32_t*)& RetData, sizeof(int32_t));
	else if (AxisNo == 65535)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
		{
			ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x00 + i), (uint32_t*)& RetData, sizeof(int32_t));
			Pos[i] = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[i];
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	else
		ret = ECAT_ERR_MC_INVALID_AXIS_NO;

	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (AxisNo < EcatDevice[DeviceNo].McAxisCnt)
			* Pos = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[AxisNo];
		else if (AxisNo == 65535)
		{
		}
		else
			ret = ECAT_ERR_MC_INVALID_AXIS_NO;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
		return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisCommandPos_Directly(uint16_t DeviceNo, uint16_t AxisNo, double* Pos)
{
	int32_t ret;
	ecat_mc_return_i32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DirectlyEnable = EcatDevice[DeviceNo].DirectlyEnable;
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DirectlyEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;

	if (AxisNo < McAxisCnt)
		ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x40 + AxisNo), (uint32_t*)& RetData, sizeof(int32_t));
	else if (AxisNo == 65535)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
		{
			ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x40 + i), (uint32_t*)& RetData, sizeof(int32_t));
			Pos[i] = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[i];
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	else
		ret = ECAT_ERR_MC_INVALID_AXIS_NO;

	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		if (AxisNo < EcatDevice[DeviceNo].McAxisCnt)
			* Pos = RetData.I32Value[0] / EcatDevice[DeviceNo].AxisPPU[AxisNo];
		else if (AxisNo == 65535){}
		else
			ret = ECAT_ERR_MC_INVALID_AXIS_NO;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisState_Directly(uint16_t DeviceNo, uint16_t AxisNo, uint32_t * State)
{
	int32_t ret;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DirectlyEnable = EcatDevice[DeviceNo].DirectlyEnable;
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DirectlyEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	if (AxisNo < McAxisCnt)
		ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x80 + AxisNo / 4), (uint32_t*)& RetData, sizeof(uint32_t));
	else if (AxisNo == 65535)
	{
		for (i = 0; i < McAxisCnt; i++)
		{
			ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x80 + i / 4), (uint32_t*)& RetData, sizeof(int32_t));
			State[i] = (RetData.U32Value[0] >> ((i % 4) * 8)) & 0xFF;
		}
	}
	else
		ret = ECAT_ERR_MC_INVALID_AXIS_NO;

	if (ret == 0)
	{
		if (AxisNo < McAxisCnt)
			* State = (RetData.U32Value[0] >> ((AxisNo % 4) * 8)) & 0xFF;
		else if (AxisNo == 65535){}
		else
			ret = ECAT_ERR_MC_INVALID_AXIS_NO;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_hontech_Directly(uint16_t DeviceNo, uint16_t AxisNo, uint32_t* DI)
{
	int32_t ret;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DirectlyEnable = EcatDevice[DeviceNo].DirectlyEnable;
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DirectlyEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	if (AxisNo < EcatDevice[DeviceNo].McAxisCnt)
		ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x90 + AxisNo/4), (uint32_t*)& RetData, sizeof(uint32_t));
	else if (AxisNo == 65535)
	{
		for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
		{
			ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0x90 + i / 4), (uint32_t*)& RetData, sizeof(int32_t));
			DI[i] = (RetData.U32Value[0] >> ((i % 4) * 8)) & 0xFF;
		}
	}
	else
		ret = ECAT_ERR_MC_INVALID_AXIS_NO;

	if (ret == 0)
	{
		if (AxisNo < EcatDevice[DeviceNo].McAxisCnt)
			* DI = (RetData.U32Value[0] >> ((AxisNo % 4) * 8)) & 0xFF;
		else if (AxisNo == 65535){}
		else
			ret = ECAT_ERR_MC_INVALID_AXIS_NO;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisDI_Directly(uint16_t DeviceNo, uint16_t AxisNo, uint32_t* DI)
{
	int32_t ret;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DirectlyEnable = EcatDevice[DeviceNo].DirectlyEnable;
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DirectlyEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	if (AxisNo < McAxisCnt)
		ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xC0 + AxisNo / 4), (uint32_t*)& RetData, sizeof(uint32_t));
	else if (AxisNo == 65535)
	{
		for (i = 0; i < McAxisCnt; i++)
		{
			ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xC0 + i / 4), (uint32_t*)& RetData, sizeof(int32_t));
			DI[i] = (RetData.U32Value[0] >> ((i % 4) * 8)) & 0xFF;
		}
	}
	else
		ret = ECAT_ERR_MC_INVALID_AXIS_NO;

	if (ret == 0)
	{
		if (AxisNo < McAxisCnt)
			* DI = (RetData.U32Value[0] >> ((AxisNo % 4) * 8)) & 0xFF;
		else if (AxisNo == 65535) {}
		else
			ret = ECAT_ERR_MC_INVALID_AXIS_NO;
	}
		return ret;
}
/*****************************************************************************/
int32_t ECAT_McEnable_Directly(uint16_t DeviceNo)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_ENABLE_DIRECTLY,
		&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].DirectlyEnable = 1;
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McGetAxisStateDI_Ex(uint16_t DeviceNo)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;
	ecat_mc_return_u32_2_t RetData;
	uint32_t RetSize;
	int i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint16_t McAxisCnt = EcatDevice[DeviceNo].McAxisCnt;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	RetSize = sizeof(ecat_mc_return_u32_u32_t) * McAxisCnt;
	Ops.No = 65535;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_AXIS_STATE_DI_EX,
		&Ops, sizeof(ecat_ipc_base_ops_t), &RetData, RetSize);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		for (i = 0; i < EcatDevice[DeviceNo].McAxisCnt; i++)
		{
			EcatDevice[DeviceNo].AxisState[i] = RetData.U3232Value[i].U32Value1;
			EcatDevice[DeviceNo].AxisDI[i] = RetData.U3232Value[i].U32Value2;
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveDIMap(uint16_t DeviceNo, uint16_t ChannelCnt, uint16_t *SlaveNo, uint16_t *Dir)
{
	int32_t ret;
	ecat_dio_map_ops_t Ops;
	size_t i;

	if (ChannelCnt > SLAVE_DIO_MAP_MAX)
	{
		return ECAT_ERR_SLAVE_CNT_EXCEEDED;
	}
	Ops.SlaveCnt = ChannelCnt;
	for (i = 0; i < ChannelCnt; i++)
	{
		Ops.SlaveNo[i] = SlaveNo[i];
		Ops.Dir[i] = Dir[i];
	}
	
	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_SLAVE_DIO_MAP,
		&Ops, sizeof(ecat_dio_map_ops_t), NULL, 0);
	if (ret == 0)
	{
		SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
		EcatDevice[DeviceNo].DIOmapEnable = 1;
		EcatDevice[DeviceNo].DIO_map_Cnt = ChannelCnt;
		for (i = 0; i < ChannelCnt; i++)
		{
			EcatDevice[DeviceNo].DIO_map_No[i] = SlaveNo[i];
			EcatDevice[DeviceNo].DIO_map_Dir[i] = Dir[i];
		}
		SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDIO(uint16_t DeviceNo)
{
	return 0;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveAiVoltData_Ex(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t ChannelCnt, double *Data)
{
	int32_t ret;
	ecat_aio_ex_ops_t Ops;
	ecat_aio_ex_return_t RetData;
	uint32_t RetSize;
	uint16_t i;
	if(ChannelCnt > SLAVE_AIO_CHANNEL_MAX)
    {
		return ECAT_ERR_INVALID_PARAM;
    }
	Ops.SlaveNo = SlaveNo;
    Ops.ChannelCnt = ChannelCnt;
    RetSize = sizeof(double) * ChannelCnt;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_AI_EX,
		&Ops, sizeof(ecat_aio_ex_ops_t), &RetData, RetSize);
    if(ret == 0)
    {
        for (i = 0; i < ChannelCnt; i++)
        {
            *(Data + i) = RetData.f64Value[i];
        }
    }
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDI_Directly(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t* Value)
{
	int32_t ret;
	uint16_t SlaveIdx;
	ecat_mc_return_u32_t RetData;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DIOmapEnable = EcatDevice[DeviceNo].DIOmapEnable;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	size_t i;

	if (DIOmapEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SlaveIdx = 65535;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	for (i = 0; i < EcatDevice[DeviceNo].DIO_map_Cnt; i++)
	{
		if ((EcatDevice[DeviceNo].DIO_map_No[i] == SlaveNo) && (EcatDevice[DeviceNo].DIO_map_Dir[i] == 0))
		{
			SlaveIdx = i;
			break;
		}
	}
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (SlaveIdx == 65535) goto out_return;
	ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xA0 + SlaveIdx), (uint32_t*)& RetData, sizeof(uint32_t));
	if (ret == 0)
	{
		*Value = RetData.U32Value[0];
	}
out_return:
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDIBit_Directly(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t* Value)
{
	int32_t ret;
	uint16_t SlaveIdx;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DIOmapEnable = EcatDevice[DeviceNo].DIOmapEnable;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DIOmapEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SlaveIdx = 65535;
	ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	for (i = 0; i < EcatDevice[DeviceNo].DIO_map_Cnt; i++)
	{
		if ((EcatDevice[DeviceNo].DIO_map_No[i] == SlaveNo) && (EcatDevice[DeviceNo].DIO_map_Dir[i] == 0))
		{
			SlaveIdx = i;
			break;
		}
	}
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (SlaveIdx == 65535) goto out_return;
	ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xA0 + SlaveIdx), (uint32_t*)& RetData, sizeof(uint32_t));
	if (ret == 0)
	{
		*Value = (RetData.U32Value[0] >> BitNo) & 0x00000001;
	}
out_return:
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDO_Directly(uint16_t DeviceNo, uint16_t SlaveNo, uint32_t* Value)
{
	int32_t ret;
	uint16_t SlaveIdx;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DIOmapEnable = EcatDevice[DeviceNo].DIOmapEnable;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DIOmapEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SlaveIdx = 65535;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	for (i = 0; i < EcatDevice[DeviceNo].DIO_map_Cnt; i++)
	{
		if ((EcatDevice[DeviceNo].DIO_map_No[i] == SlaveNo) && (EcatDevice[DeviceNo].DIO_map_Dir[i] == 1))
		{
			SlaveIdx = i;
			break;
		}
	}
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (SlaveIdx == 65535) goto out_return;
	ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xA0 + SlaveIdx), (uint32_t*)& RetData, sizeof(uint32_t));
	if (ret == 0)
	{
		*Value = RetData.U32Value[0];
	}
out_return:
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetSlaveDOBit_Directly(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t BitNo, uint32_t* Value)
{
	int32_t ret;
	uint16_t SlaveIdx;
	ecat_mc_return_u32_t RetData;
	size_t i;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	uint8_t DIOmapEnable = EcatDevice[DeviceNo].DIOmapEnable;
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (DIOmapEnable != 1)
		return ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SlaveIdx = 65535;
	ret = ECAT_ERR_MC_HAVE_NOT_BEEN_SET;
	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	for (i = 0; i < EcatDevice[DeviceNo].DIO_map_Cnt; i++)
	{
		if ((EcatDevice[DeviceNo].DIO_map_No[i] == SlaveNo) && (EcatDevice[DeviceNo].DIO_map_Dir[i] == 1))
		{
			SlaveIdx = i;
			break;
		}
	}
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if (SlaveIdx == 65535) goto out_return;
	ret = GetReturnData_Directly(&EcatDevice[DeviceNo], (0xA0 + SlaveIdx), (uint32_t*)& RetData, sizeof(uint32_t));
	if (ret == 0)
	{
		*Value = (RetData.U32Value[0] >> BitNo) & 0x00000001;
	}
out_return:
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetBuffetAmp(uint16_t DeviceNo, uint16_t BeffetNo, double Amp)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = BeffetNo;
	Ops.DoubleData[0] = Amp;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_BUFFET_AMP,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetBuffetFreq(uint16_t DeviceNo, uint16_t BeffetNo, double Freq)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = BeffetNo;
	Ops.DoubleData[0] = Freq;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_BUFFET_FREQ,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetBuffetGain(uint16_t DeviceNo, uint16_t BeffetNo, uint16_t AxisNo, double Gain)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = BeffetNo;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Gain;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_BUFFET_GAIN,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetImpactGain(uint16_t DeviceNo, uint16_t ImpactNo, uint16_t AxisNo, double Gain)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = ImpactNo;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Gain;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_IMPACT_GAIN,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
		return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetImpactFadeTime(uint16_t DeviceNo, uint16_t ImpactNo, uint16_t AxisNo, double FadeTime_s)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = ImpactNo;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = FadeTime_s;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_IMPACT_FADETIME,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetImpactFreq(uint16_t DeviceNo, uint16_t ImpactNo, uint16_t AxisNo, double Freq)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = ImpactNo;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Freq;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_IMPACT_FREQ,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetImpactDamp(uint16_t DeviceNo, uint16_t ImpactNo, uint16_t AxisNo, double Damp)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = ImpactNo;
	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Damp;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_IMPACT_DAMP,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetImpact(uint16_t DeviceNo, uint16_t ImpactNo)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.U16Data[0] = ImpactNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_IMPACT,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceXilinxVer(uint16_t DeviceNo, uint16_t* Version)
{
	int32_t ret;
	uint32_t Value;
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, CARD_ID_ADDR_OFFSET, &Value, 1) != 0))
		return ret;
	*Version = Value >> 16;
	return 0;
}
/*****************************************************************************/
int32_t ECAT_GetDeviceXilinx_Process(uint16_t DeviceNo, uint8_t* Process)
{
	*Process = EcatDevice[DeviceNo].Xilinx_Process;
	return 0;
}
/*****************************************************************************/
int32_t ECAT_WriteDeviceXilinx(uint16_t DeviceNo, char* sFileName)
{
	FILE* Update_File;
	unsigned char buff[256];
	int32_t ret = 0;
	uint32_t cnt_tmp, val_tmp;
	unsigned long file_size, file_index, file_space;
	unsigned long lFlashData, lCommandIndex;
	unsigned char Index_CRC, Buff_CRC_Lo, Buff_CRC_Hi;
	char szModelName[20] = "Unknow";

	EcatDevice[DeviceNo].Xilinx_Process = 0;

	lCommandIndex = 0x00000000;
	Update_File = fopen(sFileName, "rb");
	if (!Update_File)
	{
		ret = ECAT_ERR_OPEN_FILE;
		goto safe_return;
	}

	fseek(Update_File, 0, SEEK_END);
	file_size = ftell(Update_File);

	fseek(Update_File, 0, SEEK_SET);
	// Pass the first page for last write
	fread(buff, sizeof(buff), 1, Update_File);
	// Initial the Command Status

	uint32_t data;
	uint8_t wIndex;
	for (wIndex = 0; wIndex < 8; wIndex++)
	{
		data = STA_COMBUSY;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, wIndex * 0x20 + REG_STATUS_HOST, &data, 1);//CMD BUSY
		data = STA_PCIECHK;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, wIndex * 0x20 + REG_STATUS_DEVICE, &data, 1);//PCIE CHK
	}
	for (file_index = 0x100; file_index < file_size; file_index += 0x100)
	{
		// Save the data into Dual Port RAM
		if ((file_size - file_index) < sizeof(buff))
			file_space = file_size - file_index;
		else
			file_space = sizeof(buff);

		fread(buff, file_space, 1, Update_File);
		Buff_CRC_Hi = Buff_CRC_Lo = 0xFF;

		for (cnt_tmp = 0x0; cnt_tmp < file_space; cnt_tmp++)
		{
			Index_CRC = Buff_CRC_Lo ^ buff[cnt_tmp];
			Buff_CRC_Lo = Buff_CRC_Hi ^ CRC16Hi[Index_CRC];
			Buff_CRC_Hi = CRC16Lo[Index_CRC];
			if ((cnt_tmp % 0x04) == 0x00)
				lFlashData = buff[cnt_tmp];
			else
			{
				val_tmp = buff[cnt_tmp];
				val_tmp <<= ((cnt_tmp % 0x04) * 8);
				lFlashData += val_tmp;
				data = lFlashData;
				if (((cnt_tmp % 0x04) == 0x03) || (cnt_tmp == (file_space - 1)))
					write_bar0_reg(EcatDevice[DeviceNo].Fd, 0x500 + ((cnt_tmp / 0x04) * 0x04), &data, 1);
			}
		}

		lFlashData = file_space - 1;
		lFlashData <<= 16;
		lFlashData += Buff_CRC_Hi;
		lFlashData <<= 8;
		lFlashData += Buff_CRC_Lo;
		//printf("File Index = %08lX\n", file_index);

		// Write to Flash
		data = lCommandIndex;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COUNT, &data, 1);
		//DeviceWritePhyMemory((dwBaseAddr + REG_COUNT), lCommandIndex);
		//printf("Command Index = %08lX\n", lCommandIndex);
		if (file_index == 0x100)
		{
			data = 0xE000005C;
			write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COMMAND, &data, 1);
			//DeviceWritePhyMemory((dwBaseAddr + REG_COMMAND), 0xE000005C);
			//printf("Command = 0xE000005C\n");
		}
		else {
			data = 0xE800005C;
			write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COMMAND, &data, 1);
			//DeviceWritePhyMemory((dwBaseAddr + REG_COMMAND), 0xE800005C);
			//printf("Command Index = 0xE800005C\n");
		}

		data = lFlashData;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_DATA, &data, 1);
		//DeviceWritePhyMemory((dwBaseAddr + REG_DATA), lFlashData);
		//printf("Command Data = %08lX\n", lFlashData);
		data = STA_COMREADY;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);  // Command is ready
		//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMREADY);	// Command is ready
		//printf("Command Status = %08lX\n", STA_COMREADY);

		EcatDevice[DeviceNo].Xilinx_Process = ((float)file_index / (float)file_size) * 100;
		//printf("Update Process.... %02d %%\n", (WORD)(((float)file_index / (float)file_size) * 100));
		while (1)
		{
		RETRY:
			read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_DEVICE, &val_tmp, 1);// Read status of command
			//DeviceReadPhyMemory((dwBaseAddr + REG_STATUS_DEVICE), &val_tmp);	// Read status of command
			//printf("\nERROR CODE:%x",val_tmp);
			if (val_tmp == STA_COMVALID)	// Command Valid
			{
				data = STA_COMBUSY;
				write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);  // Command is not ready
				//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMBUSY);	// Command is not ready
				break;
			}
			else if (val_tmp != STA_BUSY && val_tmp != STA_PCIECHK) {							// Command Invalid
				switch (val_tmp)
				{
				case STA_ERRCRC:
					ret = ERR_RW_FLASH_ERRCRC;
					//printf("Error: Check CRC Error\n");
					goto err_Response;
					break;
				case STA_OVERADDRESS:
					ret = ERR_RW_FLASH_OVERADDRESS;
					//printf("Error: Over the Definition of Flash Address\n");
					goto err_Response;
					break;
				case STA_ERRCOM:
					ret = ERR_RW_FLASH_ERRCOM;
					//printf("Error: Command Error\n");
					goto err_Response;
					break;
				case STA_ERRFLASHCTL:
					ret = ERR_RW_FLASH_ERRFLASHCTL;
					//printf("Error: Flash Control Fail\n");
					goto err_Response;
					break;
				default:
					//printf("Error: Out of our Definition(%lX)\n", val_tmp);
					goto RETRY;
					break;
				}
				break;
			}
		}

		// Check the status of the command
		read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COUNT_CONF, &val_tmp, 1);
		//DeviceReadPhyMemory((dwBaseAddr + REG_COUNT_CONF), &val_tmp);
		if (val_tmp != lCommandIndex++)
		{
			ret = ERR_RW_FLASH_CFM_CMD_CNT;
			goto err_Response;
			//printf("Error Command Count:%08lX (%08lX)\n", val_tmp, (lCommandIndex - 1));
		}//else{
		//	printf("Check Command Count:%08lX\n", val_tmp);}

		read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COMMAND_CONF, &val_tmp, 1);
		//DeviceReadPhyMemory((dwBaseAddr + REG_COMMAND_CONF), &val_tmp);
		if (file_index == 0x100)
		{
			if (val_tmp != 0xE000005C)
			{
				ret = ERR_RW_FLASH_CFM_CMD;
				goto err_Response;
				//printf("Error Command:%08lX (%08lX)\n", val_tmp, 0xE000005C);

			}//else{
			//	printf("Check Command:%08lX\n", val_tmp);}
		}
		else {
			if (val_tmp != 0xE800005C)
			{
				ret = ERR_RW_FLASH_CFM_CMD;
				goto err_Response;
				//printf("Error Command:%08lX (%08lX)\n", val_tmp, 0xE800005C);
			}//else{
			//	printf("Check Command:%08lX\n", val_tmp);}
		}

		read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_DATA_CONF, &val_tmp, 1);
		//DeviceReadPhyMemory((dwBaseAddr + REG_DATA_CONF), &val_tmp);
		//printf("Check Command Data:%08lX (%08lX)\n", val_tmp, lFlashData);

		data = STA_COMBUSY;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);  // Command is not ready
		data = STA_PCIECHK;
		write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_DEVICE, &data, 1); // PCIe chack finished
		//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMBUSY);	// Command is not ready
		//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_DEVICE), STA_PCIECHK);	// PCIe chack finished

	}
	// Save the first page
	fseek(Update_File, 0, SEEK_SET);
	fread(buff, sizeof(buff), 1, Update_File);
	Buff_CRC_Hi = Buff_CRC_Lo = 0xFF;
	for (cnt_tmp = 0x0; cnt_tmp < 0x100; cnt_tmp++)
	{

		Index_CRC = Buff_CRC_Lo ^ buff[cnt_tmp];
		Buff_CRC_Lo = Buff_CRC_Hi ^ CRC16Hi[Index_CRC];
		Buff_CRC_Hi = CRC16Lo[Index_CRC];
		if ((cnt_tmp % 0x04) == 0x00)
			lFlashData = buff[cnt_tmp];
		else
		{
			val_tmp = buff[cnt_tmp];
			val_tmp <<= ((cnt_tmp % 0x04) * 8);
			lFlashData += val_tmp;
			data = lFlashData;
			if (((cnt_tmp % 0x04) == 0x03))
				write_bar0_reg(EcatDevice[DeviceNo].Fd, 0x500 + ((cnt_tmp / 0x04) * 0x04), &data, 1);
			//DeviceWritePhyMemory((dwBaseAddr + 0x500 + ((cnt_tmp / 0x04) * 0x04)), lFlashData);
		}
	}

	lFlashData = 0xFF;
	lFlashData <<= 16;
	lFlashData += Buff_CRC_Hi;
	lFlashData <<= 8;
	lFlashData += Buff_CRC_Lo;
	// Write to Flash
	data = lCommandIndex;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COUNT, &data, 1);
	data = 0xE000005D;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COMMAND, &data, 1);
	data = lFlashData;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_DATA, &data, 1);
	data = STA_COMREADY;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);// Command is ready
	//DeviceWritePhyMemory((dwBaseAddr + REG_COUNT), lCommandIndex);
	//DeviceWritePhyMemory((dwBaseAddr + REG_COMMAND), 0xE000005D);
	//DeviceWritePhyMemory((dwBaseAddr + REG_DATA), lFlashData);
	//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMREADY);	// Command is ready
	//printf("File Index = 00000000\n");
	//printf("Command Index = %08lX\n", lCommandIndex);
	while (1)
	{
		read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_DEVICE, &val_tmp, 1);// Read status of command
		//DeviceReadPhyMemory((dwBaseAddr + REG_STATUS_DEVICE), &val_tmp);	// Read status of command
		if (val_tmp == STA_COMVALID)	// Command Valid
		{
			data = STA_COMBUSY;
			write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);// Command is not ready
			//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMBUSY);	// Command is not ready
			break;

		}
		else if (val_tmp != STA_BUSY && val_tmp != STA_PCIECHK) {							// Command Invalid
			switch (val_tmp)
			{
			case STA_ERRCRC:
				ret = ERR_RW_FLASH_ERRCRC;
				//printf("Error: Check CRC Error\n");
				goto err_Response;
				break;
			case STA_OVERADDRESS:
				ret = ERR_RW_FLASH_OVERADDRESS;
				//printf("Error: Over the Definition of Flash Address\n");
				goto err_Response;
				break;
			case STA_ERRCOM:
				ret = ERR_RW_FLASH_ERRCOM;
				//printf("Error: Command Error\n");
				goto err_Response;
				break;
			case STA_ERRFLASHCTL:
				ret = ERR_RW_FLASH_ERRFLASHCTL;
				//printf("Error: Flash Control Fail\n");
				goto err_Response;
				break;
			default:
				//printf("Error: Out of our Definition(%lX)\n", val_tmp);
				goto RETRY;
				break;
			}
			break;
		}

	}

	// Check the status of the command
	read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COUNT_CONF, &val_tmp, 1);
	//DeviceReadPhyMemory((dwBaseAddr + REG_COUNT_CONF), &val_tmp);
	if (val_tmp != lCommandIndex++)
	{
		ret = ERR_RW_FLASH_CFM_CMD_CNT;
		goto err_Response;
		//printf("Error Command Count:%08lX (%08lX)\n", val_tmp, (lCommandIndex - 1));
	}//else{
	//	printf("Check Command Count:%08lX\n", val_tmp);}

	read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_COMMAND_CONF, &val_tmp, 1);
	//DeviceReadPhyMemory((dwBaseAddr + REG_COMMAND_CONF), &val_tmp);
	if (val_tmp != 0xE000005D)
	{
		ret = ERR_RW_FLASH_CFM_CMD;
		goto err_Response;
		//printf("Error Command:%08lX (%08lX)\n", val_tmp, 0xE000005D);
	}//else{
	//	printf("Check Command:%08lX\n", val_tmp);}

	read_bar0_reg(EcatDevice[DeviceNo].Fd, REG_DATA_CONF, &val_tmp, 1);
	//DeviceReadPhyMemory((dwBaseAddr + REG_DATA_CONF), &val_tmp);
	//printf("Check Command Data:%08lX (%08lX)\n", val_tmp, lFlashData);
	data = STA_COMBUSY;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);// Command is not ready
	data = STA_PCIECHK;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_DEVICE, &data, 1);// PCIe check finished

	//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMBUSY);	// Command is not ready
	//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_DEVICE), STA_PCIECHK);	// PCIe check finished

	//printf("Update Process....100 %%\n");
err_Response:
	data = STA_COMBUSY;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_HOST, &data, 1);// Command is not ready
	data = STA_PCIECHK;
	write_bar0_reg(EcatDevice[DeviceNo].Fd, REG_STATUS_DEVICE, &data, 1);// PCIe check finished

	//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_HOST), STA_COMBUSY);	// Command is not ready
	//DeviceWritePhyMemory((dwBaseAddr + REG_STATUS_DEVICE), STA_PCIECHK);	// PCIe check finished
	fclose(Update_File);
safe_return:

	//printf("\nFirmware Update OK\n");
	//printf("\n===============================================");
	//printf("\n|                 WARRING                     |");
	//printf("\n-----------------------------------------------");
	//printf("\n|You must shutdown/turn off(3 sec.) the PC and|");
	//printf("\n|turn on the PC to update the newest firmware.|");
	//printf("\n|If you only reboot PC. The newest firmware   |");
	//printf("\n|can't update successfully.                   |");
	//printf("\n===============================================\n");
	EcatDevice[DeviceNo].Xilinx_Process = 100;
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisHome_Servotronix(uint16_t DeviceNo, uint16_t MasterNo, uint16_t DiffenceNo, uint16_t Settings)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.MasterNo = MasterNo;
	Ops.AxisNo = DiffenceNo;
	Ops.U16Data[0] = Settings;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_HOME_SERVOTRONIX,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McSetAxisActualPos_Servotronix(uint16_t DeviceNo, uint16_t MasterNo, uint16_t DiffenceNo, double Position)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.MasterNo = MasterNo;
	Ops.AxisNo = DiffenceNo;
	Ops.DoubleData[0] = Position;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_SET_AXIS_POS_SERVOTRONIX,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryOut(uint16_t DeviceNo, uint16_t AxisNo)
{
	int32_t ret;
	ecat_ipc_base_ops_t Ops;

	Ops.No = AxisNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRYOUT,
		&Ops, sizeof(ecat_ipc_base_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetSlaveState(uint16_t DeviceNo, uint16_t SlaveNo, uint16_t State)
{
	int32_t ret;
	ecat_set_slave_state_ops_t Ops;

	Ops.No = SlaveNo;
	Ops.State = State;

	ret = GenericSendCommand(DeviceNo, 1000, ECAT_IPC_CMD_SET_SLAVE_STATE,
		&Ops, sizeof(ecat_set_slave_state_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_GetFoEState(uint16_t DeviceNo, uint16_t* State)
{
	int32_t ret;
	ecat_set_slave_state_ops_t RetData;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_GET_SLAVE_FOE_STATE,
		NULL, 0, &RetData, sizeof(ecat_set_slave_state_ops_t));
	if (ret == 0)
	{
		*State = RetData.State;
	}
	return ret;
}
/*****************************************************************************/
int32_t ECAT_SetDeviceCmpDisable(uint16_t DeviceNo, uint16_t CmpNo)
{
	if (CmpNo >= DEV_ENCODER_MAX)
		return ECAT_ERR_MC_INVALID_AXIS_NO;

	int32_t ret;
	uint32_t config;
	uint32_t config_offset;
	if (CmpNo == 0) {
		config_offset = ENC0_CMP_TRIG_CONFIG_ADDR_OFFSET;
	}
	else {
		config_offset = ENC1_CMP_TRIG_CONFIG_ADDR_OFFSET;
	}

	SEMEPHORE_P_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	if ((ret = read_bar0_reg(EcatDevice[DeviceNo].Fd, config_offset, &config, 1) != 0))
		goto out_return;
	config &= ~0x80000000;
	if ((ret = write_bar0_reg(EcatDevice[DeviceNo].Fd, config_offset, &config, 1) != 0))
		goto out_return;
out_return:
	SEMEPHORE_V_OPERATION(EcatDevice[DeviceNo].ThreadSem)
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisGantryGainEx(uint16_t DeviceNo, uint16_t SlaveNo, double Kp, double Ki, double Kd)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = SlaveNo;
	Ops.DoubleData[0] = Kp;
	Ops.DoubleData[1] = Ki;
	Ops.DoubleData[2] = Kd;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GANTRY_GAIN,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisTangentOut(uint16_t DeviceNo, uint16_t AxisNo, uint16_t GroupNo)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.U16Data[0] = GroupNo;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_TANGENT_OUT,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveVelEx(uint16_t DeviceNo, uint16_t AxisNo, double Vel)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.Vel = Vel;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_VEL_EX,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisMoveTorEx(uint16_t DeviceNo, uint16_t AxisNo, double Torque)
{
	int32_t ret;
	ecat_mc_axis_ops_t Ops;

	Ops.AxisNo = AxisNo;
	Ops.DoubleData[0] = Torque;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_MOVE_TOR_EX,
		&Ops, sizeof(ecat_mc_axis_ops_t), NULL, 0);
	return ret;
}
/*****************************************************************************/
int32_t ECAT_McAxisGearInByPos(uint16_t DeviceNo, uint16_t MasterNo, uint16_t SlaveNo, int32_t RatioNum, uint32_t RationDen, uint16_t SyncSource, double MasterSynPos, double SlaveSynPos, double GearOutPos, double MaterialLength, double ReturnVel, uint16_t ReturnAcctime)
{
	int32_t ret;
	ecat_mc_gearin_by_pos_ops_t Ops;

	Ops.MasterNo = MasterNo;
	Ops.AxisNo = SlaveNo;
	Ops.I32Data = RatioNum;
	Ops.U32Data = RationDen;
	Ops.U16Data[0] = SyncSource;
	Ops.MasterSyncPos = MasterSynPos;
	Ops.SlaveSyncPos = SlaveSynPos;
	Ops.GearOutPos = GearOutPos;
	Ops.MaterialLength = MaterialLength;
	Ops.ReturnVel = ReturnVel;
	Ops.ReturnAcctime = ReturnAcctime;

	ret = GenericSendCommand(DeviceNo, 0, ECAT_IPC_CMD_MC_AXIS_GEAR_IN_BY_POS,
		&Ops, sizeof(ecat_mc_gearin_by_pos_ops_t), NULL, 0);
	return ret;
}
