#define	_IN_EMBFPGA_C
#include "embfpga.h"

//----------------------------------------------------------------------------

#include "corefmc.h"
#include "inout.h"
#include "shell.h"

//----------------------------------------------------------------------------

void TestFPGA(char * para1, char * para2);

//----------------------------------------------------------------------------

// 初始化FPGA
void InitEmbFpga(void)
{
	int hardver = GetFpgaHardVersion();
	printf("\r\nFPGA Hard Version = Ver %x.%x \r\n", (hardver&0xff00)>>8,(hardver&0x00ff));

	int softver = GetFpgaSoftVersion();
	printf("\r\nFPGA Soft Version = Ver %x.%x \r\n", (softver&0xff00)>>8,(softver&0x00ff));

	SetExoutEnOn();		// 输出允许

	//AddShellCmd("FPGA", "test rw fpga", TestFPGA);

}

//----------------------------------------------------------------------------
// 读端口

// 读取硬件版本
u16 GetFpgaHardVersion(void)
{
	static u16 hardver = 0;
	if ((hardver == 0) || (hardver == 0xffff))
	{// 上电后只从FPGA中读一次
		hardver = FmcReadReg(FPGA_HARD_VERSION);
	}
	return hardver;
}

// 读取软件版本
u16 GetFpgaSoftVersion(void)
{
	static u16 softver = 0;
	if ((softver == 0) || (softver == 0xffff))
	{// 上电后只从FPGA中读一次
		softver = FmcReadReg(FPGA_SOFT_VERSION);
	}
	return softver;
}

// 得到虚轴运行状态
u16 GetRunStatus(void)
{
	SetVAxisConfig(VCTR_LOCK);		// 锁存
	return FmcReadReg(VAXIS_STATUS);
}

// 得到虚轴表状态
u16 GetRunStatus1(void)
{
	return FmcReadReg(VAXIS_STATUS1);
}

// 得到输入端口中断信息
u16 GetInputIntFlags(void)
{
	return (FmcReadReg(INPUT_INT_FLAGS) & INPUT_INT_MASK);
}

// 得到编码器中断信息
u16 GetEcdIntFlags(void)
{
	return (FmcReadReg(ECD_INT_FLAGS) & ECD_INT_MASK);
}

// 读取虚轴1的运行频率
u32 GetVAxis1PPS(void)
{
	u32 freq;
	SetVAxisConfig(VCTR_LOCK);		// 锁存
	freq = FmcReadReg(CUR_VAXIS1_PPS_H);		// 高字
	freq <<= 16;
	freq += FmcReadReg(CUR_VAXIS1_PPS_L);		// 低字
	return freq;
}

// 读取虚轴的运行频率
// vaxisId: VAXIS_ID1
u32 GetVAxisPPS(u16 vaxisId)
{
	assert_param(IS_VAXIS_ID(vaxisId));

	switch (vaxisId)
	{
	case VAXIS_ID1:
		return GetVAxis1PPS();
	default:
		return 0;
	}
}

// 读取上一个表结束时的虚轴频率
u32 GetLastVAxisFrequency(u16 vaxisId)
{
	u32 freq;
	assert_param(vaxisId == VAXIS_ID1);

	if (vaxisId == VAXIS_ID1)
	{
		freq = FmcReadReg(VAXIS1_END_FREQUENCY_H);		// 高字
		freq <<= 16;
		freq += FmcReadReg(VAXIS1_END_FREQUENCY_L);	// 低字
	}
	else
	{
		freq = 0;
	}

	return freq;
}

// 读取虚轴1的计数值
u32 GetVAxis1Counter(void)
{
	u32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_VAXIS1_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_VAXIS1_COUNTER_L);	// 低字
	return Counter;
}

// 读取虚轴的计数值
// vaxisId: VAXIS_ID1
u32 GetVAxisCounter(u16 vaxisId)
{
	assert_param(IS_VAXIS_ID(vaxisId));

	switch (vaxisId)
	{
	case VAXIS_ID1:
		return GetVAxis1Counter();
	default:
		return 0;
	}
}

// 读取实轴1计数器值
s32 GetAxis1Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS1_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS1_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴2计数器值
s32 GetAxis2Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS2_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS2_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴3计数器值
s32 GetAxis3Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS3_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS3_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴4计数器值
s32 GetAxis4Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS4_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS4_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴5计数器值
s32 GetAxis5Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS5_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS5_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴6计数器值
s32 GetAxis6Counter(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_AXIS6_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_AXIS6_COUNTER_L);	// 低字
	return Counter;
}

// 读取实轴计数器值
// axisId: AXIS_ID1 ~ AXIS_ID6
s32 GetAxisCounter(u16 axisId)
{
	assert_param(IS_AXIS_ID(axisId));

	switch (axisId)
	{
	case AXIS_ID1:
		return GetAxis1Counter();
	case AXIS_ID2:
		return GetAxis2Counter();
	case AXIS_ID3:
		return GetAxis3Counter();
	case AXIS_ID4:
		return GetAxis4Counter();
	case AXIS_ID5:
		return GetAxis5Counter();
	case AXIS_ID6:
		return GetAxis6Counter();
	default:
		return -1;
	}
}

// 得到编码器1的数值
s32 GetEcd1Value(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_ECD1_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_ECD1_COUNTER_L);		// 低字
	return Counter;
}

// 得到编码器2的数值
s32 GetEcd2Value(void)
{
	s32 Counter;
	SetVAxisConfig(VCTR_LOCK);			// 锁存
	Counter = FmcReadReg(CUR_ECD2_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_ECD2_COUNTER_L);		// 低字
	return Counter;
}

// 得到编码器的数值
// ecdId: ECD_SEL1 ~ ECD_SEL2
s32 GetEcdCounter(u16 ecdId)
{
	assert_param(IS_ECD_SEL(ecdId));

	switch (ecdId)
	{
	case ECD_SEL1:
		return GetEcd1Value();
	case ECD_SEL2:
		return GetEcd2Value();
	default:
		return -1;
	}
}

// 读取OP计数器值
u32 GetOPCounter(void)
{
	u32 Counter;
	SetVAxisConfig(VCTR_LOCK);	// 锁存
	Counter = FmcReadReg(CUR_OP_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_OP_COUNTER_L);		// 低字
	return Counter;
}

// 读取ZP计数器值
u32 GetZPCounter(void)
{
	u32 Counter;
	SetVAxisConfig(VCTR_LOCK);	// 锁存
	Counter = FmcReadReg(CUR_ZP_COUNTER_H);		// 高字
	Counter <<= 16;
	Counter += FmcReadReg(CUR_ZP_COUNTER_L);		// 低字
	return Counter;
}

// 得到实轴报警信息
u16 GetAlarmValue(void)
{
	return (FmcReadReg(AXIS_ALM_STATUS) & AXIS_ALM_MASK);
}

// 得到编码器信号状态
u16 GetEcdStatus(void)
{
	return (FmcReadReg(ECD_SIGNAL_STATUS) & ECD_STA_MASK);
}

// 得到输入信号状态
u16 GetInputStatus(void)
{
	return (FmcReadReg(INPUT_STATUS));
}

//----------------------------------------------------------------------------
// 写端口
// 设置虚轴配置
void SetVAxisConfig(u16 cfg)
{
	assert_param(IS_VAXIS_CMD(cfg));

	FmcWriteReg(VAXIS_CONTROL, cfg);
}

// 功能: 清除FPGA中断
void CleanAllIntFlag(void)
{
	SetVAxisConfig(VCTR_INPUT_ALM_INT);		// 清除input中断标志
	SetVAxisConfig(VCTR_ZP1_INT);			// 编码器1:清除ZP1中断标志
	SetVAxisConfig(VCTR_ZP2_INT);			// 编码器2:清除ZP1中断标志

	SetVAxisConfig(VCTR_VAXIS1_CLEAN_OV1);	// 虚轴1清除表1完成中断标志
	SetVAxisConfig(VCTR_VAXIS1_CLEAN_OV2);	// 虚轴1清除表2完成中断标志
}

// 启动虚轴运动
int StartVAxisRun(VAxisCmdStr * pCmd)
{
	u32 addr;

	if (pCmd == NULL)
	{
		return -1;
	}

	assert_param(IS_VAXIS_ID(pCmd->vaxisId));
	assert_param(IS_BUF_SEL(pCmd->bufSel));

	if (pCmd->vaxisId == VAXIS_ID1)		// 虚轴1
	{
		if (pCmd->bufSel == BUF_SEL1)
		{
			SetVAxisConfig(VCTR_VAXIS1_DIS_BUF1);
			addr = VAXIS1_BUF1_PARA;
		}
		else if (pCmd->bufSel == BUF_SEL2)
		{
			SetVAxisConfig(VCTR_VAXIS1_DIS_BUF2);
			addr = VAXIS1_BUF2_PARA;
		}
		else if (pCmd->bufSel == BUF_SEL3)
		{
			SetVAxisConfig(VCTR_VAXIS1_DIS_BUF3);
			addr = VAXIS1_BUF3_PARA;
		}
		else
		{
			return -1;
		}
	}
	else
	{
		return -1;
	}

	if ((pCmd->vaxisId == VAXIS_ID1) ||		// 虚轴1
			0	)
	{
		if (pCmd->calcGap > MAX_GAP)
		{
//			printf("calcGap=%d > MAX_GAP\r\n", pCmd->calcGap);
			pCmd->calcGap = MAX_GAP;
		}
		else if (pCmd->calcGap < MIN_GAP)
		{
//			printf("calcGap=%d < MIN_GAP\r\n", pCmd->calcGap);
			pCmd->calcGap = MIN_GAP;
		}

		if (pCmd->startPPS > MAX_PPS)
		{
			printf("startPPS=%d > MAX_PPS\r\n", pCmd->startPPS);
			pCmd->startPPS = MAX_PPS;
		}
		else if (pCmd->startPPS < MIN_PPS)
		{
			printf("startPPS=%d < MIN_PPS\r\n", pCmd->startPPS);
			pCmd->startPPS = MIN_PPS;
			return -1;		// 修改防止运动太慢，认为死机
		}

		if (pCmd->runPPS > MAX_PPS)
		{
			printf("runPPS=%d > MAX_PPS\r\n", pCmd->runPPS);
			pCmd->runPPS = MAX_PPS;
		}
		else if (pCmd->runPPS < MIN_PPS)
		{
			printf("runPPS=%d < MIN_PPS\r\n", pCmd->runPPS);
			pCmd->runPPS = MIN_PPS;
			return -1;		// 修改防止运动太慢，认为死机
		}

		FmcWriteReg(addr, LOWORD(pCmd->calcGap));
		FmcWriteReg(addr, HIWORD(pCmd->calcGap));		// 计算时间间隔，1/72 us 为单位
		FmcWriteReg(addr, LOWORD(pCmd->startPPS));
		FmcWriteReg(addr, HIWORD(pCmd->startPPS));		// 启动频率，单位pps
		FmcWriteReg(addr, LOWORD(pCmd->runPPS));
		FmcWriteReg(addr, HIWORD(pCmd->runPPS));		// 目标频率，单位pps

		if (pCmd->bufSel != BUF_SEL3)
		{
			assert_param(pCmd->outNum <= MAX_OUTNUM);
			assert_param(pCmd->outNum >= MIN_OUTNUM);
			assert_param(pCmd->jumpNum <= MAX_OUTNUM);

			FmcWriteReg(addr, LOWORD(pCmd->outNum));
			FmcWriteReg(addr, HIWORD(pCmd->outNum));	// 本次输出虚轴脉冲个数，单位个
			FmcWriteReg(addr, LOWORD(pCmd->jumpNum));
			FmcWriteReg(addr, HIWORD(pCmd->jumpNum));	// 变速跳过虚轴脉冲个数
			FmcWriteReg(addr, VOK_FLAG);				// OK
		}
		else
		{
			FmcWriteReg(addr, VOK_FLAG);
		}

		// 虚轴运行
		if (pCmd->vaxisId == VAXIS_ID1)
		{
//			printf("vaxis 1 run cmd\r\n");
			SetVAxisConfig(VCTR_VAXIS1_RUN);
		}
		else
		{
		}
	}
	else
	{
		return -1;
	}

	return 0;
}


// 虚轴:停止当前运动
int StopVAxisRun(u16 vaxisId)
{
	assert_param(IS_VAXIS_ID(vaxisId));

	//虚轴停止
	if (vaxisId == VAXIS_ID1 || vaxisId == VAXIS_ID_ALL)
	{
		SetVAxisConfig(VCTR_VAXIS1_STOP);
		SetVAxisConfig(VCTR_VAXIS1_CLEAN_OV1);
		SetVAxisConfig(VCTR_VAXIS1_CLEAN_OV2);
		SetVAxisConfig(VCTR_VAXIS1_DIS_BUF1);
		SetVAxisConfig(VCTR_VAXIS1_DIS_BUF2);
		SetVAxisConfig(VCTR_VAXIS1_DIS_BUF3);
	}

	return 0;
}

// 虚轴:速度解除速度临时控制
void StopSpdCtrl(u16 vaxisId)
{
	assert_param(IS_VAXIS_ID(vaxisId));

	if (vaxisId == VAXIS_ID1 || vaxisId == VAXIS_ID_ALL)
	{
		SetVAxisConfig(VCTR_VAXIS1_DIS_BUF3);
	}
}

// 编码器的数值清零
// ecdId: ECD_SEL1 ~ ECD_SEL2
void ClearEcdCounter(u16 ecdId)
{
	assert_param(IS_ECD_SEL(ecdId));

	switch (ecdId)
	{
	case ECD_SEL1:
		SetVAxisConfig(VCTR_ECD1_CLEAR);
		break;
	case ECD_SEL2:
		SetVAxisConfig(VCTR_ECD2_CLEAR);
		break;
	default:
		break;
	}
}

void ReetEcdABZLevel(u16 ecdId)
{
	switch (ecdId)
	{
	case ECD_SEL1:
		SetVAxisConfig(VCTR_ECD1_LEVEL_RST);
		break;
	case ECD_SEL2:
		SetVAxisConfig(VCTR_ECD1_LEVEL_RST);
		break;
	default:
		break;
	}
}

void SetEcdAPLevelNegative(u16 ecdId)
{
	switch (ecdId)
	{
	case ECD_SEL1:
		SetVAxisConfig(VCTR_AP1_LEVEL_NEG);
		break;
	case ECD_SEL2:
		SetVAxisConfig(VCTR_AP2_LEVEL_NEG);
		break;
	default:
		break;
	}
}

void SetEcdAZLevelNegative(u16 ecdId)
{
	switch (ecdId)
	{
	case ECD_SEL1:
		SetVAxisConfig(VCTR_AZ1_LEVEL_NEG);
		break;
	case ECD_SEL2:
		SetVAxisConfig(VCTR_AZ2_LEVEL_NEG);
		break;
	default:
		break;
	}
}

void SetEcdZPLevelNegative(u16 ecdId)
{
	switch (ecdId)
	{
	case ECD_SEL1:
		SetVAxisConfig(VCTR_ZP1_LEVEL_NEG);
		break;
	case ECD_SEL2:
		SetVAxisConfig(VCTR_ZP2_LEVEL_NEG);
		break;
	default:
		break;
	}
}

// OP输出
void SetOpOutOn(void)
{
	SetVAxisConfig(VCTR_OPOS_ON);
}

void SetOpOutOff(void)
{
	SetVAxisConfig(VCTR_OPOS_NONE);
}

void SetZpOutOn(void)
{
	SetVAxisConfig(VCTR_ZPOS_ON);
}

void SetZpOutOff(void)
{
	SetVAxisConfig(VCTR_ZPOS_NONE);
}

// 设置报警命令
void SetAxisAlarmConfig(u16 axisId, u16 alm)
{
	assert_param(IS_AXIS_ID(axisId));

	SetVAxisConfig(MAKE_VCTR_AXIS_ALM_CMD(axisId,alm));
}

//--------------------------------------------------------------------------------------------------------------

// 设置OP输出的分频系数
void SetOpDivision(u16 div)
{
	if (div != OP_OUT_DIRECT)
	{
		div &= 0xFFFE;
	}
	FmcWriteReg(OP_OUT_DIVISION, div);
}

// 设置OP计数值
void SetOpCounter(s32 cnt)
{
	FmcWriteReg(OP_COUNTER_SET_L, LOWORD(cnt));
	FmcWriteReg(OP_COUNTER_SET_H, HIWORD(cnt));
}

// 设置ZP计数值
void SetZpCounter(s32 cnt)
{
	FmcWriteReg(ZP_COUNTER_SET_L, LOWORD(cnt));
	FmcWriteReg(ZP_COUNTER_SET_H, HIWORD(cnt));
}

void SetEcd1Counter(s32 cnt)
{
	FmcWriteReg(ECD1_COUNTER_SET_L, LOWORD(cnt));
	FmcWriteReg(ECD1_COUNTER_SET_H, HIWORD(cnt));
}

void SetEcd2Counter(s32 cnt)
{
	FmcWriteReg(ECD2_COUNTER_SET_L, LOWORD(cnt));
	FmcWriteReg(ECD2_COUNTER_SET_H, HIWORD(cnt));
}

// 设置编码器计数值
void SetEcdCounter(u16 ecdId, s32 cnt)
{
	assert_param(IS_ECD_SEL(ecdId));

	switch(ecdId)
	{
		case ECD_SEL1:
		{
			SetEcd1Counter(cnt);
			break;
		}
		case ECD_SEL2:
		{
			SetEcd2Counter(cnt);
			break;
		}
		default:
			break;
	}
}

// 设置输入端口中断配置
void SetInputInt(u16 cfg)
{
	FmcWriteReg(INPUT_INT_SET, cfg);
}

#define MAX_OUTPUT_NUM			8

u16 g_outputValue = 0;

void Output1On(void)
{
	g_outputValue |= OUTPUT1_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output1Off(void)
{
	g_outputValue &= (~OUTPUT1_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output2On(void)
{
	g_outputValue |= OUTPUT2_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output2Off(void)
{
	g_outputValue &= (~OUTPUT2_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output3On(void)
{
	g_outputValue |= OUTPUT3_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output3Off(void)
{
	g_outputValue &= (~OUTPUT3_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output4On(void)
{
	g_outputValue |= OUTPUT4_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output4Off(void)
{
	g_outputValue &= (~OUTPUT4_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output5On(void)
{
	g_outputValue |= OUTPUT5_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output5Off(void)
{
	g_outputValue &= (~OUTPUT5_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output6On(void)
{
	g_outputValue |= OUTPUT6_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output6Off(void)
{
	g_outputValue &= (~OUTPUT6_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output7On(void)
{
	g_outputValue |= OUTPUT7_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output7Off(void)
{
	g_outputValue &= (~OUTPUT7_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output8On(void)
{
	g_outputValue |= OUTPUT8_BIT;
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

void Output8Off(void)
{
	g_outputValue &= (~OUTPUT8_BIT);
	FmcWriteReg(OUTPUT_SET, g_outputValue);
}

// 得到某个输出口状态
u8 GetOutputVal(int num)
{
	if (num > 0 && num <= MAX_OUTPUT_NUM)
	{
		u16 mod = 0x01;
		mod <<= (num-1);
		if ((g_outputValue & mod) != 0)
		{
			return SENSOR_ON;
		}
		else
		{
			return SENSOR_OFF;
		}
	}

	return SENSOR_OFF;
}

//-------------------------------------------------------------

// 设置实轴输出配置
void SetAxisConfig(u16 axisId, u16 cfg)
{
	assert_param(IS_AXIS_ID(axisId));
	assert_param(IS_AXIS_CMD(cfg));

	FmcWriteReg(AXIS_CONFIG(AXIS(axisId)), cfg);
}

u32 GetSineNums(u16 sineout, u16* pBuffer, u32 num)
{
	u32 sinenum, temp;
	sinenum = 0;

	if (sineout != 0)
	{
		while(num != 0)
		{
			temp = (*pBuffer) & 0x7fff;
			sinenum += temp;

			//temp = (u32)(temp * SINE_MUTI + 0.5);
			temp = (u32)(temp * SINE_MUTI + 0.999f);	// 20220407 ljs 李帅 王春刚 修改,修复在走少量脉冲时丢脉冲的现象
			if (temp > 0x7fff)
			{
				printf("sine calc error\r\n");
				sinenum += 0x100000;	// 返回大于 0xfffff 的数，以便在调用函数中报错
			}

			*pBuffer &= 0x8000;
			*pBuffer |= (temp & 0x7fff);

			num--;
			pBuffer++;
		}
	}

	return sinenum;
}

// 写插补数据
// 返回值: 0:正常
//		-1，输入参数错误
//		-2，保留,不允许使用
//		-3，fpga 读写错误
// 		-4，FPGA 版本错误
int WriteInterData(InterData * pCmd)
{
	u32 sineNum;

	if (pCmd == NULL)
	{
		printf("para is empty in WriteInterData\r\n");
		return -1;
	}

	assert_param(IS_VAXIS_ID(pCmd->vaxisId));
	assert_param(IS_AXIS_ID(pCmd->axisId));
	assert_param(IS_BUF_SEL(pCmd->bufSel));

	assert_param(pCmd->datBufLen < MAX_SEGMENT_PER_BUF);
	assert_param(pCmd->datBufLen > 0);
	assert_param(pCmd->startEscNum <= MAX_OUTNUM);
	assert_param(pCmd->partVxNum <= MAX_OUTNUM);
	assert_param(pCmd->partVxNum >= MIN_OUTNUM);

	// 选择虚轴
	switch (pCmd->vaxisId)
	{
	case VAXIS_ID1:
		SetAxisConfig((pCmd->axisId), POUT_ENABLE_VAXIS1); 	// 脉冲输出允许:输入时钟为虚轴1
		break;
	case VAXIS_ECD:
		SetAxisConfig((pCmd->axisId), POUT_ENABLE_ECD); 	// 脉冲输出允许:输入为编码器接口
		break;
	default:
		SetAxisConfig((pCmd->axisId), POUT_DISABLE); 		// 脉冲输出禁止√
		break;
	}

	sineNum = GetSineNums(pCmd->sineOut, pCmd->datBuff, pCmd->datBufLen);

	// 配置实轴参数
	if (pCmd->bufSel == BUF_SEL1)
	{
//		printf("\r\nwrite buff 1\r\n");
		SetAxisConfig((pCmd->axisId), DIS_INTER_BUF1); 	// 清空表

		// 正弦输出模式
		FmcWriteReg(AXIS_BUF1_SINE_OUT(AXIS(pCmd->axisId)), (u16)(sineNum & 0xffff));		// 写入正弦数据低位
		FmcWriteReg(AXIS_BUF1_SINE_OUT_H(AXIS(pCmd->axisId)), (u16)(sineNum>>16));			// 写入正弦数据高位

		FmcWriteConstBuf(INTER_BUF1_DATA(AXIS(pCmd->axisId)), pCmd->datBuff, pCmd->datBufLen);	// 写入数据
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->startEscNum));	// 起始跳过实轴脉冲数
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->startEscNum));
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->partVxNum));		// 每段对应虚轴脉冲数
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->partVxNum));
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->dirAttr));		// 实轴脉冲输出方向
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->dirAttr));
		FmcWriteReg(INTER_BUF1_PARA(AXIS(pCmd->axisId)), VOK_FLAG);
	}
	else if (pCmd->bufSel == BUF_SEL2)
	{
//		printf("\r\nwrite buff 2\r\n");
		SetAxisConfig((pCmd->axisId), DIS_INTER_BUF2); 	// 清空表

		// 正弦输出模式
		FmcWriteReg(AXIS_BUF2_SINE_OUT(AXIS(pCmd->axisId)), (u16)(sineNum & 0xffff));			// 写入正弦数据低位
		FmcWriteReg(AXIS_BUF2_SINE_OUT_H(AXIS(pCmd->axisId)), (u16)(sineNum>>16));			// 写入正弦数据高位

		FmcWriteConstBuf(INTER_BUF2_DATA(AXIS(pCmd->axisId)), pCmd->datBuff, pCmd->datBufLen);	// 写入数据
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->startEscNum));	// 起始脉冲数
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->startEscNum));
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->partVxNum));		// 每段脉冲数
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->partVxNum));
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), LOWORD(pCmd->dirAttr));		// 方向属性
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), HIWORD(pCmd->dirAttr));
		FmcWriteReg(INTER_BUF2_PARA(AXIS(pCmd->axisId)), VOK_FLAG);
	}
	else
	{
		printf("bufSel = %d, error\r\n", pCmd->bufSel);
		return -1;
	}

	return 0;
}

void CleanAxisBuff(u16 axisId, u16 bufSel)
{
	assert_param(IS_AXIS_ID(axisId));
	assert_param(IS_BUF_SEL(bufSel));

	if (bufSel == BUF_SEL1)
	{
//		printf("clean axis %d buff 1\r\n", axisId);
		SetAxisConfig(axisId, DIS_INTER_BUF1); 	// 清空表
		FmcWriteConstBuf(INTER_BUF1_DATA(AXIS(axisId)), NULL, 0);	// 写入空数据
	}
	else if (bufSel == BUF_SEL2)
	{
//		printf("clean axis %d buff 2\r\n", axisId);
		SetAxisConfig(axisId, DIS_INTER_BUF2); 	// 清空表
		FmcWriteConstBuf(INTER_BUF2_DATA(AXIS(axisId)), NULL, 0);	// 写入空数据
	}
}

// 设置实轴计数器初值
void SetAxisCounter(u16 axisId, s32 cnt)
{
	assert_param(IS_AXIS_ID(axisId));

	FmcWriteReg(AXIS_COUNTER_L(AXIS(axisId)), LOWORD(cnt));
	FmcWriteReg(AXIS_COUNTER_H(AXIS(axisId)), HIWORD(cnt));
}

// 设置轴PWM分频系数
void SetAxisPwmFreq(u16 axisId, u16 freq)
{
	assert_param(IS_AXIS_PWM_ID(axisId));
	assert_param(IS_AXIS_PWM_FREQ(freq));

	FmcWriteReg(AXIS_PWM_FREQ(AXIS(axisId)), freq);
}

// 设置轴PULSE的PWM值
void SetAxisPulsePwm(u16 axisId, u16 val)
{
	if (val > FPGA_PWM_MAX_VAL)
	{
		val = FPGA_PWM_MAX_VAL;
	}
	assert_param(IS_AXIS_PWM_ID(axisId));
	assert_param(IS_AXIS_PWM_VAL(val));

	FmcWriteReg(AXIS_PULSE_PWM(AXIS(axisId)), val);
}

// 设置轴SIGN的PWM值
void SetAxisSignPwm(u16 axisId, u16 val)
{
	if (val > FPGA_PWM_MAX_VAL)
	{
		val = FPGA_PWM_MAX_VAL;
	}
	assert_param(IS_AXIS_PWM_ID(axisId));
	assert_param(IS_AXIS_PWM_VAL(val));

	FmcWriteReg(AXIS_SIGN_PWM(AXIS(axisId)), val);
}

//--------------------------------------------------------------------

typedef void (*InputIntProc)(void);

InputIntProc g_inputIntProcAry[16] = {NULL};

void RegInputIntProc(u16 intcfg, void(*inputProc)(void))
{
	static u16 intconfig = 0;
	if (intcfg == INPUT1_INT_EN)
	{
		g_inputIntProcAry[0] = inputProc;
	}
	else if (intcfg == INPUT2_INT_EN)
	{
		g_inputIntProcAry[1] = inputProc;
	}
	else if (intcfg == INPUT3_INT_EN)
	{
		g_inputIntProcAry[2] = inputProc;
	}
	else if (intcfg == INPUT4_INT_EN)
	{
		g_inputIntProcAry[3] = inputProc;
	}
	else if (intcfg == INPUT5_INT_EN)
	{
		g_inputIntProcAry[4] = inputProc;
	}
	else if (intcfg == INPUT6_INT_EN)
	{
		g_inputIntProcAry[5] = inputProc;
	}
	else if (intcfg == INPUT7_INT_EN)
	{
		g_inputIntProcAry[6] = inputProc;
	}
	else if (intcfg == INPUT8_INT_EN)
	{
		g_inputIntProcAry[7] = inputProc;
	}
	else if (intcfg == INPUT9_INT_EN)
	{
		g_inputIntProcAry[8] = inputProc;
	}
	else if (intcfg == INPUT10_INT_EN)
	{
		g_inputIntProcAry[9] = inputProc;
	}
	else if (intcfg == INPUT11_INT_EN)
	{
		g_inputIntProcAry[10] = inputProc;
	}
	else if (intcfg == INPUT12_INT_EN)
	{
		g_inputIntProcAry[11] = inputProc;
	}
	else if (intcfg == INPUT13_INT_EN)
	{
		g_inputIntProcAry[12] = inputProc;
	}
	else if (intcfg == INPUT14_INT_EN)
	{
		g_inputIntProcAry[13] = inputProc;
	}
	else if (intcfg == INPUT15_INT_EN)
	{
		g_inputIntProcAry[14] = inputProc;
	}
	else if (intcfg == INPUT16_INT_EN)
	{
		g_inputIntProcAry[15] = inputProc;
	}

	intconfig |= intcfg;
	SetInputInt(intconfig);
}

InputIntProc g_ecdIntProcAry[2] = {NULL};

void RegEcdInputProc(u16 ecdId, void(*ecdProc)(void))
{
	assert_param(IS_ECD_SEL(ecdId));

	if (ecdId == ECD_SEL1)
	{
		g_ecdIntProcAry[0] = ecdProc;
	}
	else if (ecdId == ECD_SEL2)
	{
		g_ecdIntProcAry[1] = ecdProc;
	}
}

// 外部中断响应函数
void FpgaIntProc(void)
{
	u16 tmp16;
	{
		tmp16 = GetRunStatus();		// 读到运行状态

		if ((tmp16 & INPUT_ALARM_FLAG) != 0)		// 输入信号下降沿中断
		{
			//printf("Get input int, GetRunStatus=0x%x, ", tmp16);
			tmp16 = GetInputIntFlags();
			//printf("GetInputIntFlags=0x%x\r\n", tmp16);

			SetVAxisConfig(VCTR_INPUT_ALM_INT);		// 清除中断标志

			if ((tmp16 & INPUT01_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[0] != NULL)
				{
					g_inputIntProcAry[0]();
				}
			}
			if ((tmp16 & INPUT02_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[1] != NULL)
				{
					g_inputIntProcAry[1]();
				}			
			}
			if ((tmp16 & INPUT03_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[2] != NULL)
				{
					g_inputIntProcAry[2]();
				}
			}
			if ((tmp16 & INPUT04_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[3] != NULL)
				{
					g_inputIntProcAry[3]();
				}
			}
			if ((tmp16 & INPUT05_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[4] != NULL)
				{
					g_inputIntProcAry[4]();
				}
			}
			if ((tmp16 & INPUT06_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[5] != NULL)
				{
					g_inputIntProcAry[5]();
				}
			}
			if ((tmp16 & INPUT07_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[6] != NULL)
				{
					g_inputIntProcAry[6]();
				}
			}
			if ((tmp16 & INPUT08_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[7] != NULL)
				{
					g_inputIntProcAry[7]();
				}
			}
			if ((tmp16 & INPUT09_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[8] != NULL)
				{
					g_inputIntProcAry[8]();
				}
			}
			if ((tmp16 & INPUT10_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[9] != NULL)
				{
					g_inputIntProcAry[9]();
				}
			}
			if ((tmp16 & INPUT11_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[10] != NULL)
				{
					g_inputIntProcAry[10]();
				}
			}
			if ((tmp16 & INPUT12_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[11] != NULL)
				{
					g_inputIntProcAry[11]();
				}
			}
			if ((tmp16 & INPUT13_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[12] != NULL)
				{
					g_inputIntProcAry[12]();
				}
			}
			if ((tmp16 & INPUT14_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[13] != NULL)
				{
					g_inputIntProcAry[13]();
				}
			}
			if ((tmp16 & INPUT15_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[14] != NULL)
				{
					g_inputIntProcAry[14]();
				}
			}
			if ((tmp16 & INPUT16_INT_FLAG) != 0)
			{
				if (g_inputIntProcAry[15] != NULL)
				{
					g_inputIntProcAry[15]();
				}
			}
		}

		if ((tmp16 & ENCODER_ALARM_FLAG) != 0)		// 编码器ZP信号上升沿中断
		{
			//printf("Get zp int, GetRunStatus=0x%x, ", tmp16);
			tmp16 = GetEcdIntFlags();
			//printf("GetInputIntFlags=0x%x\r\n", tmp16);

			if ((tmp16 & ECD1ZP_INT_FLAG) != 0)
			{
				SetVAxisConfig(VCTR_ZP1_INT);		// 清除编码器1 ZP 中断标志

				if (g_ecdIntProcAry[0] != NULL)
				{
					g_ecdIntProcAry[0]();
				}
			}

			if ((tmp16 & ECD2ZP_INT_FLAG) != 0)
			{
				SetVAxisConfig(VCTR_ZP2_INT);		// 清除编码器2 ZP 中断标志

				if (g_ecdIntProcAry[1] != NULL)
				{
					g_ecdIntProcAry[1]();
				}
			}
		}
	}
}

//--------------------------------------------------------------------
u16 g_exOutBuf[EXIO_PORT_NUM] = {0xffff};

void SetExoutEnOn(void)
{
	SetVAxisConfig(EXOUT_EN_ON);
}
void SetExoutEnOff(void)
{
	SetVAxisConfig(EXOUT_EN_OFF);
}

// 设置值
void SetExOutValue(u16 addr, u8 value)
{
	if (addr == 0 || addr > MAX_EXIO_ADDR)
	{
		return;
	}

	if (value == Bit_SET)
	{
		g_exOutBuf[(addr-1)/EXIO_PER_PORT] |= EXIO_ADDR_MOD(addr-1);
	}
	else
	{
		g_exOutBuf[(addr-1)/EXIO_PER_PORT] &= ~(EXIO_ADDR_MOD(addr-1));
	}
}

u8 GetExOutValue(u16 addr)
{
	if (addr == 0 || addr > MAX_EXIO_ADDR)
	{
		return 0xff;
	}

	if ((g_exOutBuf[(addr-1)/EXIO_PER_PORT] & EXIO_ADDR_MOD(addr-1)) != 0)
	{
		return Bit_SET;
	}
	return Bit_RESET;
}

void FlushExOutputs(void)
{
	u16 addr;
	for (addr = 0; addr < MAX_EXIO_ADDR; addr += EXIO_PER_PORT)
	{
		FmcWriteReg(EXIO_PORT_ADDR(addr), g_exOutBuf[(addr)/EXIO_PER_PORT]);
	}
}


// 设置值并输出
void SetExOutput(u16 addr, u8 value)
{
	SetExOutValue(addr, value);

#if (0)
	// 只刷新输出端口
	FsmcWriteReg(EXIO_PORT_ADDR(addr-1), g_exOutBuf[(addr-1)/EXIO_PER_PORT]);
#else
	// 刷新所有输出端口
	FlushExOutputs();
#endif
}

// 读取值
u8 GetExInput(u16 addr)
{
	if (addr == 0 || addr > MAX_EXIO_ADDR)
	{
		return Bit_SET;
	}

	return (((FmcReadReg(EXIO_PORT_ADDR(addr-1)) & EXIO_ADDR_MOD(addr-1)) != 0) ? Bit_SET : Bit_RESET);
}

void GetExInputs(u16 * pBuf)
{
	int i;

	if (pBuf == NULL)
	{
		return;
	}

	for (i = 0; i < EXIO_PORT_NUM; i++)
	{
		pBuf[i] = FmcReadReg(EXIO_PORT_ADDR(i*EXIO_PER_PORT));
	}
}

void TestFPGA(char * para1, char * para2)
{
	int p1, p2;

	if (para1 == NULL || para2 == NULL)
	{
		return;
	}

	printf("para1=%s, para2=%s\r\n", para1, para2);

	p1 = 0;
	p2 = 0;
	if (strcmp(para1, "") != 0)
	{
		p1 = atoi(para1);
	}
	if (strcmp(para2, "") != 0)
	{
		p2 = atoi(para2);
	}

	if (p1 == p2)
	{
	}

	if (p1 == -1)
	{
		u16 val = FmcReadReg(VAXIS+p2);
		printf("read addr 0x%x, val=0x%x\r\n", p2, val);
	}
	else if (p1 >= 0)
	{
		FmcWriteReg(VAXIS+p1, p2);
		printf("write addr 0x%x, val=0x%x\r\n", p1, p2);
	}
	else if (p1 == -2)
	{
		int hardver = GetFpgaHardVersion();
		printf("\r\nFPGA Hard Version = Ver %x.%x \r\n", (hardver&0xff00)>>8,(hardver&0x00ff));

		int softver = GetFpgaSoftVersion();
		printf("\r\nFPGA Soft Version = Ver %x.%x \r\n", (softver&0xff00)>>8,(softver&0x00ff));
	}
	else if (p1 == -3)
	{
		printf("op counter=%d\r\n", GetOPCounter());
	}
	else if (p1 == -4)
	{
		printf("zp counter=%d\r\n", GetZPCounter());
	}
	else if (p1 == -5)
	{
		if (p2 == 1)
		{
			ClearEcdCounter(ECD_SEL1);
			printf("ClearEcd1Counter\r\n");
		}
		else if (p2 == 2)
		{
			ClearEcdCounter(ECD_SEL2);
			printf("ClearEcd2Counter\r\n");
		}
	}
	else if (p1 == -6)
	{
		SetOpCounter(p2);
		printf("SetOpCounter %d\r\n", p2);
	}
	else if (p1 == -7)
	{
		SetZpCounter(p2);
		printf("SetZpCounter %d\r\n", p2);
	}
	else if (p1 == -8)
	{
		SetEcdCounter(ECD_SEL1, p2);
		printf("SetEcd1Counter %d\r\n", p2);
	}
	else if (p1 == -9)
	{
		SetEcdCounter(ECD_SEL2, p2);
		printf("SetEcd2Counter %d\r\n", p2);
	}
}