#define	_IN_MOTOS_C

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

#include "motos.h"
#include "stm32motos.h"
#include "inout.h"

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

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

const u16 g_axisIDList[AXIS_NUM] = 
{
	AXIS_ID1,
	AXIS_ID2,
	AXIS_ID3,
	AXIS_ID4,
	AXIS_ID5,
	AXIS_ID6,	
};

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

// 初始化电机
void InitMotos(void)
{
	InitSTM32Motos();

#if (STM32_MOTOS_NUM >= 1)
	InitStm32Moto1OutFunc(SetMoto1PulseOn, SetMoto1PulseOff, SetMoto1SignOn, SetMoto1SignOff);
#endif

#if (STM32_MOTOS_NUM >= 2)
	InitStm32Moto2OutFunc(SetMoto2PulseOn, SetMoto2PulseOff, SetMoto2SignOn, SetMoto2SignOff);
#endif

#if (STM32_MOTOS_NUM >= 3)
	InitStm32Moto3OutFunc(SetMoto3PulseOn, SetMoto3PulseOff, SetMoto3SignOn, SetMoto3SignOff);
#endif

#if (STM32_MOTOS_NUM >= 4)
	InitStm32Moto4OutFunc(SetMoto4PulseOn, SetMoto4PulseOff, SetMoto4SignOn, SetMoto4SignOff);
#endif

#if (STM32_MOTOS_NUM >= 5)
	InitStm32Moto5OutFunc(SetMoto5PulseOn, SetMoto5PulseOff, SetMoto5SignOn, SetMoto5SignOff);
#endif

#if (STM32_MOTOS_NUM >= 6)
	InitStm32Moto6OutFunc(SetMoto6PulseOn, SetMoto6PulseOff, SetMoto6SignOn, SetMoto6SignOff);
#endif

	memset(&g_motosPara, 0, sizeof(MotosPara));		// 设置电机参数
}

// 读取电机位置
int GetMotosCounter(s32 * position)
{
	int i;

	if (position != NULL)
	{
		for (i = 0; i < AXIS_NUM; i++)
		{
			position[i] = GetAxisCounter(g_axisIDList[i]);
		}
	}

	return 0;
}

// 设置电机位置
void SetMotosCounter(s32 * position)
{
	int i;
	for (i = 0; i < AXIS_NUM; i++)
	{
		if (position != NULL)
		{
			SetAxisCounter(g_axisIDList[i], position[i]);
		}
		else
		{
			SetAxisCounter(g_axisIDList[i], 0);
		}
	}
}

// 刷新坐标
void RefreshMotosPos(s32 * poslist)
{
	// 全部刷新
	int i;

	for (i = 0; i < AXIS_NUM; i++)
	{
		g_motosPara.motosPos[i] = poslist[i];
	}
	g_motosPara.enFlag = DATA_VALID;
}

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

s32 GetMotoCounter(int axisIdx)
{
	assert_param(axisIdx < AXIS_NUM);
	return GetAxisCounter(g_axisIDList[axisIdx]);
}

void SetMotoCounter(int axisIdx, s32 pos)
{
	assert_param(axisIdx < AXIS_NUM);
	SetAxisCounter(g_axisIDList[axisIdx], pos);
}

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

//-------------------------------------------------------------------------------
void MotoServoCtrl(int axisIdx, int onoff)
{
	assert_param(onoff == SERVO_ON || onoff == SERVO_OFF);

	if (axisIdx < AXIS_NUM)
	{
		SetAxisConfig(g_axisIDList[axisIdx], onoff);
	}
}

// 设置电机配置
void SetMotosConfig(MotosConfig * pConfig)
{
	int i;
	u16 axisID;
	if (pConfig == NULL)
	{
		return;
	}

	if (pConfig->newConfig == 0)
	{
		return;
	}

	// 停止运行
	StopVAxisRun(VAXIS_ID_ALL);

	for (i = 0; i < AXIS_NUM; i++)
	{
		axisID = g_axisIDList[i];

		if ((pConfig->axisConfig[i].axisConfig & CONFIG_EN) != 0)		// 配置允许
		{
			if ((pConfig->axisConfig[i].axisConfig & POA_EN) != 0)			// A允许输出
			{
				SetAxisConfig(axisID, POUTA_EN);
			}
			else
			{
				SetAxisConfig(axisID, POUTA_DIS);
			}

			if ((pConfig->axisConfig[i].axisConfig & POB_EN) != 0)			// B允许输出
			{
				SetAxisConfig(axisID, POUTB_EN);
			}
			else
			{
				SetAxisConfig(axisID, POUTB_DIS);
			}

			if ((pConfig->axisConfig[i].axisConfig & SON_EN) != 0)			// 伺服ON允许
			{
				SetAxisConfig(axisID, SERVO_ON);
			}
			else
			{
				SetAxisConfig(axisID, SERVO_OFF);
			}

			if ((pConfig->axisConfig[i].axisConfig & ALM_LEVH) != 0)		// 报警电平为高
			{
				SetAxisAlarmConfig(axisID, ALM_LEV_HIGH);
			}
			else
			{
				SetAxisAlarmConfig(axisID, ALM_LEV_LOW);
			}

			if ((pConfig->axisConfig[i].axisConfig & ALM_EN) != 0)			// 报警停车允许
			{
				SetAxisAlarmConfig(axisID, ALM_STOP_EN);
			}
			else
			{
				SetAxisAlarmConfig(axisID, ALM_STOP_DIS);
			}

			if ((pConfig->axisConfig[i].axisConfig & POUT_EN) != 0)			// 输出允许
			{
				SetAxisConfig(axisID, pConfig->axisConfig[i].poutType);		// 脉冲输出模式 脉冲方式输出
				SetAxisConfig(axisID, pConfig->axisConfig[i].spdSource);	// 实轴速度控制模式(虚轴1，虚轴2，虚轴3，编码器)
				SetAxisConfig(axisID, pConfig->axisConfig[i].datSource);	// 实轴数据获取模式(硬件插补，编码器随动，编码器映射)
			}
			else
			{
				SetAxisConfig(axisID, POUT_DISABLE);		// 禁止脉冲输出
			}
		}
	}

	SetVAxisConfig(VCTR_AXIS_ALM_INT);	// 清除报警状态
}

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

// 计算加减速需要的位移
// 参数: pps1 速度1
// 		 pps2 速度2
//		 calcTime 速度变化时间，单位为1/72us
// 返回:
//		位移值
// 条件，速度变化量固定为1ppg
u32 CalcDisplacement(u32 pps1, u32 pps2, u32 calcTime)
{
	// 速度变化量固定为1ppg
	// 公式: d=(v2+v1)*(v2-v1)/(2a)
	//		 v1 = pps1
	//		 v2 = pps2
	// 		 a = 1ppg/(calcTime/FPGA_CLK)
	// 	最终 d=(v2+v1)*(v2-v1)*calcTime/(2*FPGA_CLK)
	double temp;
	if (pps1 < pps2)
	{
		temp = (pps2-pps1);
	}
	else
	{
		temp = (pps1-pps2);
	}
	temp *= (pps1+pps2);
	temp *= calcTime;
	temp /= (2*FPGA_CLK);
	return (u32)(temp+0.5);
}


// 功能：计算某位移下速度变化需要的时间间隔参数（单位为1/72us）
// 参数：
//		 disp 位移
//		 pps1 速度1
// 		 pps2 速度2
//
// 返回:
//		位移值
// 条件，速度变化量固定为1ppg

u32 CalcGapTime(u32 disp, u32 pps1, u32 pps2)
{
	double temp;
	// 速度变化量固定为1ppg
	// 公式: a=(v2+v1)*(v2-v1)/(2*d)
	//		 v1 = pps1
	//		 v2 = pps2
	//		 d = disp
	// 		 a = 1ppg/(calcTime/FPGA_CLK)
	// 	最终 calcTime= (2*FPGA_CLK)*d/((v2+v1)*(v2-v1))

	if (disp == 0 || pps1 == pps2)
	{
		return 0;
	}
	if (pps1 < pps2)
	{
		temp = (pps2-pps1);
	}
	else
	{
		temp = (pps1-pps2);
	}
	temp *= (pps1+pps2);
	temp = (2.0*FPGA_CLK)/temp;
	temp *= disp;
	return (u32)(temp+0.5);
}

// 计算某位移和某加速度下，最终的速度值
// 参数：
//		 disp 位移
//		 pps1 速度1
// 		 addgap 时间间隔
//
// 返回:
//		位移值
// 条件，速度变化量固定为1ppg

u32 CalcEndSpd(u32 disp, u32 pps1, s32 addgap)
{
	double temp;
	// 速度变化量固定为1ppg
	// 公式: a=(v2+v1)*(v2-v1)/(2*d)
	//		 v1 = pps1
	//		 d = disp
	// 		 a = 1ppg/(addgap/FPGA_CLK)
	// 	最终 pps2 = sqrt(2*FPGA_CLK*disp/addgap + pps1*pps1)

	temp = 2.0*FPGA_CLK*disp/addgap;
	temp += 1.0*pps1*pps1;
	if (temp > 0)
	{
		temp = sqrt(temp);
	}
	else
	{
		temp = 0;
	}

	return (u32)(temp+0.5);
}

//--------------------------------------------------------------------------------------------------------------------------------------------------------------
// 功能：多轴直线插补运动控制(硬件实现)，最多支持6轴直线插补，支持虚轴1或虚轴2控制
// 参数：插补控制结构指针
// 结果: 0，运动执行完成。
//		-1，输入参数错误
//		-2，执行过程中填充数据不及时错误退出
//		-3 fpga 读写错误
// 		-4 FPGA 版本错误
//		=1, 驱动器报警停车
//		=2, 紧急停止条件触发停车
//		=3, 快速停止条件触发停车
//		=4, 普通停车条件触发停车

#if (1)

int InterpolationMotion(InterMoveCtrl * pCtrl)
{
	int i, rslt;
	u32 totalnum, circlenum, circlesize, lastsize;		// 运动总量，循环总次数，切换表个数，一般表的长度，最后一个表的长度
	u32 runcircle, execcircle, runrepeat, exerepeat;	// 运动段数，执行段数，运动重复次数，执行重复次数
	u32 runcount;
	u32 pulsePerSegment;
	u16 vAxisId;
	u16 vAxisCleanOv1, vAxisCleanOv2;
	u16 vAxisBuf1Finish, vAxisBuf2Finish;
	u16 vAxisBuf1Running, vAxisBuf2Running;
//	u16 vAxisBuf1En, vAxisBuf2En;
	u16 vAxisDisBuf1, vAxisDisBuf2;

	u32 movement[AXIS_NUM], count[AXIS_NUM];
	s32 val32, position[AXIS_NUM];
	int dir[AXIS_NUM];				// 各个轴的运动方向
	int runsta, bufsta, bufsel;
	int slowSta;
	int brksta, inbrk;		// 刹车状态，正在刹车标志
	int nstpsta, innstp;	// 刹车状态，正在刹车标志
	int stopfill;			// 停止填充数据标志
	u16 tmp16, vrsta, err;
	double calc, len;
	u16 outNum;

	u32 startpps, runpps, slowpps, stoppps;		// 速度
	u32 addppsg, decppsg, brkppsg, tslppsg;		// 加速度
	u32 num, addnum, decnum, jumpnum;			// 加减速需要的脉冲个数
	u32 curtab1num, curtab2num;

	VAxisCmdStr vAxisCmd;
	InterData idata;

	num = 0;

	// ------------------------------------------------------------------------
	// 参数合法性判断
	if (pCtrl == NULL)
	{
		printf("para error in MotosRun\r\n");
		rslt = -1;		// 输入参数错误
		return rslt;
	}

	if (pCtrl->vAxisId != VAXIS_ID1)
	{
		printf("para vAxisId error in MotosRun\r\n");
		rslt = -1;		// 输入参数错误
		return rslt;		
	}

	for (i = 0; i < AXIS_NUM; i++)	// 统一实轴速度来源
	{
		pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
	}

	// 选择参与电机
	SetMotosConfig(&(pCtrl->motosConfig));

	// 等待驱动器执行
	if (pCtrl->Delay != NULL && pCtrl->motosConfig.newConfig != 0)
	{
		pCtrl->Delay(100);
	}

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

	if (pCtrl->GetEmergencyStop != NULL)	// 紧急停止条件函数
	{
		err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->errInfo = err;
			printf("emergency stop 1 in InterpolationMotion, err=0x%x\r\n", err);
			rslt = 2;		// 紧急停止条件触发停车，立刻停止脉冲发送
			return rslt;
		}
	}

	if (pCtrl->GetQuickStop != NULL)		// 出错停止条件函数
	{
		err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->errInfo = err;
			printf("quick stop 1 in InterpolationMotion, err=0x%x\r\n", err);
			rslt = 3;		// 快速停止条件触发停车
			return rslt;
		}
	}

	if (pCtrl->GetNormalStop != NULL)		// 一般停止
	{
		err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->errInfo = err;
			printf("normal stop 1 in InterpolationMotion, err=0x%x\r\n", err);
			rslt = 4;		// 普通停车条件触发停车
			return rslt;
		}
	}

	// 速度控制参数
	startpps = pCtrl->spdPara.startPPS;
	runpps = pCtrl->spdPara.runPPS;
	slowpps = pCtrl->spdPara.slowPPS;
	stoppps = pCtrl->spdPara.stopPPS;
	addppsg = pCtrl->spdPara.addPPSG;
	decppsg = pCtrl->spdPara.decPPSG;
	brkppsg = pCtrl->spdPara.brkPPSG;
	tslppsg = pCtrl->spdPara.brkPPSG*TOSLOWMUTI;

	if (startpps == 0 || runpps == 0 || slowpps == 0 || stoppps == 0 || addppsg == 0 || decppsg == 0 || brkppsg == 0 || tslppsg == 0)
	{
		printf("one of speed para is zero \r\n");
		printf("startpps = %d, runpps = %d, slowpps = %d, stoppps = %d, addppsg = %d, decppsg = %d, brkppsg = %d\r\n", startpps, runpps, slowpps, stoppps, addppsg, decppsg, brkppsg);

		rslt = -1;
		return rslt;
	}

	vAxisId = pCtrl->vAxisId;

	if (vAxisId == VAXIS_ID1)
	{
		vAxisCleanOv1 = VCTR_VAXIS1_CLEAN_OV1;
		vAxisCleanOv2 = VCTR_VAXIS1_CLEAN_OV2;

		vAxisBuf1Finish = VAXIS1_BUF1_FINISH;
		vAxisBuf2Finish = VAXIS1_BUF2_FINISH;

		vAxisBuf1Running = VAXIS1_BUF1_RUNNING;
		vAxisBuf2Running = VAXIS1_BUF2_RUNNING;

//		vAxisBuf1En = VAXIS1_BUF1_ENABLE;
//		vAxisBuf2En = VAXIS1_BUF2_ENABLE;

		vAxisDisBuf1 = VCTR_VAXIS1_DIS_BUF1;
		vAxisDisBuf2 = VCTR_VAXIS1_DIS_BUF2;
	}
	else
	{
		printf("not support vaxis\r\n");
	}
	// 计算最大位移
	totalnum = 0;		// 脉冲个数
	circlenum = 0;		// 周期数
	circlesize = 0;		// 每个循环周期的数量
	lastsize = 0;		// 最后一个周期的数量

	runrepeat = pCtrl->mvmtPara.extraRepeat;
	exerepeat = pCtrl->mvmtPara.extraRepeat;

	// 得到实轴最大位移，作为最长轴
	for (i = 0; i < AXIS_NUM; i++)
	{
		if (pCtrl->mvmtPara.movement[i] >= 0)
		{
			movement[i] = (u32)(pCtrl->mvmtPara.movement[i]);
			dir[i] = 1;
		}
		else
		{
			movement[i] = (u32)(-(pCtrl->mvmtPara.movement[i]));
			dir[i] = -1;
		}

//		printf("axis %d movement = %d.\r\n", i+1, movement[i]);

		if (totalnum < movement[i])
		{
			totalnum = movement[i];
		}
/*
		if (pCtrl->interLongSel == LAS_COMPOSITE)	// 几个轴的合成长度
		{
			calc = movement[i];
			calc *= calc;
			len += calc;
		}
		else
		{

		}
*/
		count[i] = 0;
	}

	totalnum = pCtrl->mvmtPara.interLong;
	pulsePerSegment = pCtrl->mvmtPara.pulsePerSegment;
/*
	if (pCtrl->interLong == LAS_COMPOSITE)	// 几个轴的合成长度
	{
		len = sqrt(len);
		totalnum = (u32)(len+0.5);
	}
*/
	if (totalnum == 0)
	{
		printf("move count = 0. run over\r\n");
		return -1;
	}
	else if (totalnum < 2)
	{
		circlenum = 1;
		circlesize = totalnum;
		lastsize = totalnum;
	}
	else if (totalnum < pulsePerSegment*2)	// 小于2个最大位图周期, 将其分为均匀的两个部分
	{
		circlenum = 2;
		circlesize = (totalnum)/2;
		lastsize = totalnum-circlesize;
	}
	else
	{
		circlenum = (totalnum + pulsePerSegment-1)/pulsePerSegment;
		circlesize = pulsePerSegment;
		lastsize = totalnum-circlesize*(circlenum-1);
	}

	
#if (0)
	if (pCtrl->stopMode == STOP_MODE_TAB)	// 整点停止模式下，长轴数据大于一个表时，必须为整表
	{
		if (lastsize != circlesize && totalnum > )
		{
			printf("totalnum is not equal to circle nun\r\n");
			return 0;
		}
	}
#endif

//	printf("calc para. circlenum=%d, circlesize=%d, lastsize=%d\r\n", circlenum, circlesize, lastsize);

	if (circlenum == 0 || circlesize == 0 || lastsize == 0)
	{
		printf("calc error. circlenum=%d, circlesize=%d, lastsize=%d\r\n", circlenum, circlesize, lastsize);
		return -1;
	}

	// 速度控制参数
	addnum = CalcDisplacement(startpps, runpps, addppsg);	// 加速脉冲数
	decnum = CalcDisplacement(stoppps, runpps, decppsg);	// 减速脉冲数

//	printf("before run, start=%d, stop=%d, runing=%d, \r\n\taddppsg=%d, addnum=%d, decppsg=%d, decnum=%d\r\n",startpps, stoppps, runpps, addppsg, addnum, decppsg, decnum);

	if ((totalnum < addnum + decnum) || 
		((totalnum < (addnum + decnum + pulsePerSegment)) && (addnum%pulsePerSegment) != 0) || 
		0	)
	{
		// 重新计算 加速脉冲个数，最高速度，和减速脉冲个数
		calc = addnum;
		calc /= (addnum + decnum);
		calc *= totalnum;			// 按照比例，加速脉冲的个数
		calc /= pulsePerSegment;	// 加速圈数
		addnum = (u32)calc;
		if (addnum != 0)
		{
			addnum *= pulsePerSegment;	// 取整的脉冲数
		}
		else
		{
			addnum = totalnum / 2;
		}

		len = addnum*2;
		len *= FPGA_CLK;
		len /= addppsg;
		len -= startpps;
		calc = startpps;
		calc *= startpps;
		len += calc;
		calc = sqrt(len);			// 加速脉冲数
		runpps = (u32)(calc+0.5);
		decnum = CalcDisplacement(stoppps, runpps, decppsg);	// 重新计算减速脉冲数

//		printf("recalc addnum=%d, runpps=%d, decnum=%d\r\n", addnum, runpps, decnum);
	}

	// 控制循环
	runsta = 0;
	bufsel = 0;
	bufsta = 0;
	slowSta = 0;
	brksta = 0;
	inbrk = 0;
	nstpsta = 0;
	innstp = 0;
	stopfill = 0;				// 停止填充数据标志

	runcircle = circlenum;		// 计算计数
	execcircle = circlenum;		// 执行计数
	runcount = 0;
	jumpnum = 0;
	pCtrl->errInfo = 0;
	curtab1num = 0;
	curtab2num = 0;
	rslt = 0;

	if (pCtrl->WorkBeforeRun != NULL)
	{
		pCtrl->WorkBeforeRun(pCtrl->workPara1, pCtrl->workPara2);
	}

	SetVAxisConfig(VCTR_AXIS_ALM_INT);	// 清除报警状态

	memset(&idata, 0, sizeof(InterData));

	while(execcircle > 0 || exerepeat > 0)	// 执行未完成
	{
		if ( stopfill == 0 &&						// 允许填充数据
			(bufsta & 0x03) != 0x03 && 				// 有buff空闲
			(runcircle != 0 || runrepeat != 0) && 	// 数据未完成
			1	)
		{
			if (runcircle == 1)		// 最后一圈计算
			{
				num = lastsize;
				if (runrepeat == 0)
				{
					runsta = 1;			// 最后一圈减速
				}
			}
			else
			{
				num = circlesize;
				if (runrepeat == 0 && (runsta == 0 && decnum > lastsize))	// 如果减速需要脉冲个数大于最后一圈的位移
				{
					if ((runcircle-1) <= (decnum-lastsize+circlesize-1)/circlesize)
					{
						runsta = 1;		// 减速标志
					}
				}
			}
			runcount += num;

			if (runsta == 1)	// 计算减速点
			{
				if (decnum > lastsize)
				{
					jumpnum = num - (decnum - lastsize) % num;
				}
				else
				{
					jumpnum = lastsize - decnum;
				}
				if (jumpnum >= num)
				{
					jumpnum = 0;
				}
				if (decnum != 0)
				{
					decnum = decnum - (num - jumpnum);
				}
//				printf("jumpnum=%d, decnum=%d\r\n", jumpnum, decnum);
			}

			for (i = 0; i < AXIS_NUM; i++)		// 写各个轴的插补数据
			{
				if (movement[i] != 0)
				{
					if (bufsel == 0)
					{
						idata.bufSel = BUF_SEL1;
					}
					else
					{
						idata.bufSel = BUF_SEL2;
					}

					// 计算该段插补需要的实轴数据
					//--------------------------
					idata.vaxisId = vAxisId;			// 使用虚轴1
					if (dir[i] >= 0)	// 方向
					{
						idata.dirAttr = DIR_POSPOS;	// 正向
					}
					else
					{
						idata.dirAttr = DIR_NEGNEG;	// 反向
					}
					idata.datBufLen = 1+1;				// 有效段个数: 1

					idata.axisId = g_axisIDList[i];
					idata.startEscNum = 0;
					idata.partVxNum = num;
					calc = movement[i];
					calc *= runcount;
					calc /= totalnum;
					calc -= count[i];
					outNum = (u16)(calc+0.5);
					count[i] += outNum;
					idata.datBuff[0] = outNum;			// 第一段数据
					idata.datBuff[1] = 0;				// 结束段写0
					idata.sineOut = 0;

					//printf("%d.datBuff[0]=%d\r\n", i+1, idata.datBuff[0]);
					rslt = WriteInterData(&idata);
					if (rslt < 0)
					{
						return rslt;
					}

//					printf("axis %d wt, c=%d, ac=%d\r\n", i+1, outNum, count[i]);

					if (runcount >= totalnum)
					{
						count[i] = 0;
					}
				}
				else
				{
					if (bufsel == 0)
					{
						CleanAxisBuff(g_axisIDList[i], BUF_SEL1);
					}
					else
					{
						CleanAxisBuff(g_axisIDList[i], BUF_SEL2);
					}
				}
			}
			if (runcount >= totalnum)
			{
				runcount = 0;
			}

			// 启动运动, 虚轴命令
			vAxisCmd.vaxisId = vAxisId;		// 使用虚轴
			if (bufsel == 0)
			{
				bufsta |= 0x01;
				vAxisCmd.bufSel = BUF_SEL1;
				curtab1num = num;
//				printf("write buff1\r\n");
			}
			else
			{
				bufsta |= 0x02;
				vAxisCmd.bufSel = BUF_SEL2;
				curtab2num = num;
//				printf("write buff2\r\n");
			}

			if (runsta == 0)
			{
//				printf("write add vtable\r\n");

				vAxisCmd.startPPS = startpps;		// 启动速度
				vAxisCmd.runPPS = runpps;			// 运行速度
				vAxisCmd.jumpNum = 0;				// 跳过脉冲数
				vAxisCmd.calcGap = addppsg;			// 计算时间
			}
			else
			{
//				printf("write dec vtable, jump=%d\r\n", jumpnum);

				vAxisCmd.startPPS = stoppps;		// 启动速度
				vAxisCmd.runPPS = stoppps;			// 运行速度
				vAxisCmd.jumpNum = jumpnum;			// 跳过脉冲数
				vAxisCmd.calcGap = decppsg;			// 计算时间
			}

			vAxisCmd.outNum = num;					// 表的脉冲个数

			StartVAxisRun(&vAxisCmd);				// 写运行命令

			// 得到虚轴运行状态
//			printf("At%d, StartVAxisRun, vrsta=0x%x, vrsta1=0x%x\r\n", Get100UsSoftTimer(), GetRunStatus(), GetRunStatus1());
			runcircle--;			// 计算计数减小
			if (runcircle == 0)
			{
				if (runrepeat != 0)
				{
					runrepeat--;
					runcircle = circlenum;
				}
			}
			bufsel = ~bufsel;		// 切换buf
		}

		// 等待运动结束
		rslt = 0;
		do
		{
			err = 0;
			//----------
			if (slowSta != 0 && slowpps != pCtrl->spdPara.slowPPS)
			{
				printf("reset slowsta when speed change\r\n");
				slowSta = 0;
			}
			slowpps = pCtrl->spdPara.slowPPS;		// 更新降速速度
			//----------

			{
				int count = 100;
				do
				{
					// 得到虚轴运行状态
					tmp16 = GetRunStatus();
					vrsta = GetRunStatus();

//					printf("1. run sta =0x%x\r\n", vrsta);

					while(tmp16 != vrsta)	// 稳定的值
					{
//						printf("1. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
						tmp16 = vrsta;
						vrsta = GetRunStatus();
					}

					if (	(vrsta & (vAxisBuf1Finish|vAxisBuf1Running)) != (vAxisBuf1Finish|vAxisBuf1Running) &&
							(vrsta & (vAxisBuf2Finish|vAxisBuf2Running)) != (vAxisBuf2Finish|vAxisBuf2Running)	)	// 完成标志和运行标志不可能同时有效，这时有可能是读取错误
					{
						break;
					}

					printf("1. rd sta err, sta=0x%x, count=%d\r\n", vrsta, 100-count);

				}while(count--);

				if (count == 0)
				{
					printf("rd fpga data timout\r\n");
					StopVAxisRun(vAxisId);
					rslt = -3;		// fpga 读写错误
					break;
				}


				if ((vrsta & vAxisBuf1Finish) != 0)		// 虚轴缓冲区1数据执行完成标志
				{
					SetVAxisConfig(vAxisCleanOv1);			// 清除表1完成标志
					bufsta &= 0xFE;		// 释放buf1
					execcircle--;
					if (execcircle == 0)
					{
						if (exerepeat != 0)
						{
							exerepeat--;
							execcircle = circlenum;
						}
					}
					curtab1num = 0;
				//	printf("At%d, table 1 ok, vrsta=0x%x, vsta=0x%x, vrsta1=0x%x\r\n", GetUsSoftTimer(), vrsta, GetRunStatus(), GetRunStatus1());
				}
				if ((vrsta & vAxisBuf2Finish) != 0)		// 虚轴缓冲区2数据执行完成标志
				{
					SetVAxisConfig(vAxisCleanOv2);			// 清除表2完成标志
					bufsta &= 0xFD;		// 释放buf2
					execcircle--;
					if (execcircle == 0)
					{
						if (exerepeat != 0)
						{
							exerepeat--;
							execcircle = circlenum;
						}
					}
					curtab2num = 0;

				//	printf("At%d, table 2 ok, vrsta=0x%x, vsta=0x%x, vrsta1=0x%x\r\n", GetUsSoftTimer(), vrsta, GetRunStatus(), GetRunStatus1());
				}

				if ((vrsta & AXIS_ALARM_FLAG) != 0)		// 实轴报警标志
				{
					err = GetAlarmValue();				// 得到轴报警信息
					SetVAxisConfig(VCTR_AXIS_ALM_INT);	// 清除状态
					StopVAxisRun(vAxisId);
					if (err != 0)
					{
						printf("1. axis alarm value=0x%x\r\n", err);
					}
					rslt = 1;		// 驱动器报警停车
					break;
				}
				else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0)	// 两个缓冲区的数据都执行完成，没有有效数据
				{
#if (0)
					temp16 = GetRunStatus1();
					if ((temp16 & (vAxisBuf1En|vAxisBuf2En)) == 0)
#endif
					{
					//	printf("At%d, no more data in two buff in InterpolationMotion\r\n", GetUsSoftTimer());
						StopVAxisRun(vAxisId);
						if (pCtrl->stopMode == STOP_MODE_TAB)
						{
//							printf("stop at tab end\r\n");

							if (brksta == 1)
							{
								rslt = 3;	// 快速停止条件触发停车，执行完一个缓冲区数据
							}
							else // if (nstpsta == 1)
							{
								rslt = 4;	// 普通停车条件触发停车，执行完一个缓冲区数据
							}
						}
						else
						{
//							printf("stop when spd low, execcircle=%d,exerepeat=%d\r\n", execcircle, exerepeat);
							if (execcircle == 0 && exerepeat == 0)
							{
								/*
								 * 20221124
								 * 数据执行完成的优先级高
								 * 如果停止条件触发,但降速完成后数据恰好执行完成,则认为是正常停车,而非停止条件停车
								*/
								rslt = 0;
							}
							else
							{
								rslt = -2;				// 无有效数据，退出
							}
						}
						break;
					}
				}
			}

			// 停止条件判断
			if (pCtrl->GetEmergencyStop != NULL)	// 急停条件函数
			{
				err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
				if (err != 0)
				{
					pCtrl->errInfo = err;
					StopVAxisRun(vAxisId);
					printf("1. emergency stop 3, err=0x%x\r\n", err);
					rslt = 2;		// 紧急停止条件触发停车，立刻停止脉冲发送
					break;
				}
			}

			if (inbrk != 0 || (brksta != 1 && pCtrl->GetQuickStop != NULL))		// 快停条件函数
			{
				if (inbrk == 0)
				{
					err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
					if (err != 0)
					{
						pCtrl->errInfo = err;
//						printf("quick stop in InterpolationMotion, err=0x%x\r\n", err);
					}
				}

				if (err != 0 || inbrk != 0)
				{
					// 计算从当前速度降速到停止需要的位移
					if (pCtrl->stopMode == STOP_MODE_TAB)
					{
						u32 curpps, curmvmt, decCount, resdcount;
						curpps = GetVAxisPPS(vAxisId);					// 当前速度
						decCount = CalcDisplacement(curpps, stoppps, brkppsg);	// 计算以当前运行速度减速到停止速度需要的位移
						curmvmt = GetVAxisCounter(vAxisId);				// 当前表运行进度

						// 运行状态
						tmp16 = GetRunStatus();
						vrsta = GetRunStatus();
						while(tmp16 != vrsta)
						{
//							printf("3. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
							tmp16 = vrsta;
							vrsta = GetRunStatus();
						}

//						printf("cur vaxis counter=%d, vrsta=0x%x\r\n", curmvmt, vrsta);
						resdcount = 0;
						if (decCount > (curtab1num+curtab2num-curmvmt))	// 减速需要脉冲数大于已填充数据
						{
							inbrk = 1;		// 设置进入降速状态，但需要继续填充数据和计算减速点
						}
						else
						{
							// 判断当前运行的表是那个表
							if ((vrsta & vAxisBuf1Running) != 0)			// 表1正在执行
							{
								if (decCount < (curtab1num-curmvmt))
								{
									resdcount = (curtab1num-curmvmt);
									SetVAxisConfig(vAxisDisBuf2);			// 取消掉表2的已填充数据
								}
							}
							else if ((vrsta & vAxisBuf2Running) != 0)	// 表2正在执行
							{
								if (decCount < (curtab2num-curmvmt))
								{
									resdcount = (curtab2num-curmvmt);
									SetVAxisConfig(vAxisDisBuf1);			// 取消掉表1的已填充数据
								}
							}

							if (resdcount == 0)
							{
								resdcount = (curtab1num+curtab2num-curmvmt);
							}

							brkppsg = CalcGapTime(decCount*num, curpps, stoppps);	// 按照新位移，重新计算刹车加速度

							inbrk = 0;		// 不再进入
							stopfill = 1;	// 不再填充数据
						}
					}

					// 立即降速
					brksta = 1;						// 设置刹车车状态

					vAxisCmd.vaxisId = vAxisId;
					vAxisCmd.bufSel = BUF_SEL3;
					vAxisCmd.calcGap = brkppsg;
					vAxisCmd.runPPS = stoppps;
					vAxisCmd.startPPS = stoppps;		// 进入快速降速，停止过程
					StartVAxisRun(&vAxisCmd);
				}
			}

			if (innstp != 0 || nstpsta != 1)		// 一般停止条件函数
			{
				if (innstp == 0 && nstpsta != 1)
				{
					if (pCtrl->GetNormalStop != NULL)
					{
						err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
						if (err != 0)
						{
							pCtrl->errInfo = err;
//							printf("normal stop 2 in InterpolationMotion, err=0x%x\r\n", err);
						}
					}
				}

				if (err != 0 || innstp != 0)
				{
					// 计算从当前速度降速到停止需要的位移
					if (pCtrl->stopMode == STOP_MODE_TAB)
					{
						u32 curpps, curmvmt, decCount, resdcount;
						curpps = GetVAxisPPS(vAxisId);					// 当前速度
						decCount = CalcDisplacement(curpps, stoppps, decppsg);	// 计算以当前运行速度减速到停止速度需要的位移
						curmvmt = GetVAxisCounter(vAxisId);				// 当前表运行进度
						// 运行状态

						tmp16 = GetRunStatus();
						vrsta = GetRunStatus();
						while(tmp16 != vrsta)
						{
//							printf("4. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
							tmp16 = vrsta;
							vrsta = GetRunStatus();
						}						
//						printf("cur vaxis counter=%d, vrsta=0x%x\r\n", curmvmt, vrsta);
						resdcount = 0;
						if (decCount > (curtab1num+curtab2num-curmvmt))	// 减速需要脉冲数大于已填充数据
						{
							innstp = 1;		// 设置进入降速状态，但需要继续填充数据和计算减速点
						}
						else
						{
							// 判断当前运行的表是那个表
							if ((vrsta & vAxisBuf1Running) != 0)			// 表1正在执行
							{
								if (decCount < (curtab1num-curmvmt))
								{
									resdcount = (curtab1num-curmvmt);
									SetVAxisConfig(vAxisDisBuf2);			// 取消掉表2的已填充数据
								}
							}
							else if ((vrsta & vAxisBuf2Running) != 0)	// 表2正在执行
							{
								if (decCount < (curtab2num-curmvmt))
								{
									resdcount = (curtab2num-curmvmt);
									SetVAxisConfig(vAxisDisBuf1);			// 取消掉表1的已填充数据
								}
							}

							if (resdcount == 0)
							{
								resdcount = (curtab1num+curtab2num-curmvmt);
							}

							decppsg = CalcGapTime(decCount*num, curpps, stoppps);	// 按照新位移，重新计算刹车加速度

							innstp = 0;		// 不再进入
							stopfill = 1;	// 不再填充数据
						}
					}

					// 立即降速
					nstpsta = 1;						// 设置停车状态

					vAxisCmd.vaxisId = vAxisId;
					vAxisCmd.bufSel = BUF_SEL3;
					vAxisCmd.calcGap = decppsg;
					vAxisCmd.runPPS = stoppps;
					vAxisCmd.startPPS = stoppps;		// 进入快速降速，停止过程
					StartVAxisRun(&vAxisCmd);
				}
			}

			if (pCtrl->GetSlowDown != NULL)			// 降速
			{
				tmp16 = pCtrl->GetSlowDown(pCtrl->condPara1, pCtrl->condPara2);
				if (tmp16 != 0)
				{
					if (slowSta == 0)	// 非降速状态
					{
						slowSta = 1;	// 设置为降速状态
//						printf("in slow down sta, slowpps=%d, tslppsg=%d\r\n", slowpps, tslppsg);
						vAxisCmd.vaxisId = vAxisId;		//使用虚轴
						vAxisCmd.bufSel = BUF_SEL3;
						vAxisCmd.calcGap = tslppsg;
						vAxisCmd.runPPS = slowpps;
						vAxisCmd.startPPS = slowpps;
						StartVAxisRun(&vAxisCmd);		// 处理降速
					}
				}
				else
				{
					if (slowSta == 1)	// 在降速状态
					{
						slowSta = 0;
					//	printf("out slow down sta\r\n");
						StopSpdCtrl(vAxisId);			// 恢复正常速度
					}
				}
			}

			// 刹车
			if ((brksta == 1 || nstpsta == 1) && pCtrl->stopMode == STOP_MODE_SPD)	// 停止刹车状态，且是速度低于停止频率立刻停车模式
			{
				val32 = GetVAxisPPS(vAxisId);	// 读取当前虚轴的运行频率
				if (val32 <= stoppps)			// 频率小于停止频率，立刻停止
				{
					if (brksta == 1)
					{
						rslt = 3;	// 快速停止条件触发停车，速度低于停车速度
					}
					else // if (nstpsta == 1)
					{
						rslt = 4;	// 普通停车条件触发停车，速度低于停车速度
					}
					break;
				}
				else
				{
//					printf(">stoppps\r\n");
				}
			}

			// 更新坐标
			if (GetMotosCounter(position) == 0)
			{
				RefreshMotosPos(position);			// 更新各个轴的位置
				if (pCtrl->RefreshPosition != NULL)
				{
					pCtrl->RefreshPosition(pCtrl->refPosPara, position);
				}
			}
			else
			{
				rslt = 5;
			}

		}while(0);	// exerepeat == 0 && execcircle == 1);

		if (rslt != 0)
		{
			break;
		}
	}

	StopVAxisRun(vAxisId);

	if (pCtrl->WorkAfterRun != NULL)
	{
		pCtrl->WorkAfterRun(pCtrl->workPara1, pCtrl->workPara2);
	}

	if (pCtrl->Delay != NULL)
	{
	//	pCtrl->Delay(100);
	}

	// 更新坐标
	if (GetMotosCounter(position) == 0)
	{
		RefreshMotosPos(position);			// 更新各个轴的位置

		if (pCtrl->RefreshPosition != NULL)
		{
			pCtrl->RefreshPosition(pCtrl->refPosPara, position);
		}
	}
	else
	{
		rslt = 5;
	}

	if (rslt == 0)
	{
		pCtrl->errInfo = 0;
	}

	return rslt;
}

#endif

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


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

// 功能：多轴连续直线插补运动控制，最多支持6轴的运动。 支持虚轴1或虚轴2控制
// 参数：连续插补控制结构指针
// 结果: 0，运动执行完成。
//		-1，输入参数错误
//		-2，执行过程中填充数据不及时错误退出
// 		-3 FPGA 读写错误
// 		-4 FPGA 版本错误
//		=1, 驱动器报警停车
//		=2, 紧急停止条件触发停车
//		=3, 快速停止条件触发停车
//		=4, 普通停车条件触发停车
//		=5,电机坐标获取错误

#if (1)

int ContinuousInterpolation(ContinuousInterMoveCtrl * pCtrl)
{
	int i, j, rslt, err;
	int bufsta, bufsel;		// 缓冲区状态,缓冲区选择
	u16 vrsta, tmp16;		// 虚轴运行状态
	u16 vAxisId;
	u32 decPPSG, decCount;

	u16 vAxisCleanOv1, vAxisCleanOv2;
	u16 vAxisBuf1Finish, vAxisBuf2Finish;		// 虚轴缓冲区1完成标志,虚轴缓冲区2完成标志
	u16 vAxisBuf1Running, vAxisBuf2Running;		// 虚轴缓冲区1正在执行标志,虚轴缓冲区2正在执行标志
//	u16 vAxisDisBuf1, vAxisDisBuf2;

	s32 position[AXIS_NUM];

	VAxisCmdStr vAxisCmd;
	InterData idata;

	// ------------------------------------------------------------------------
	// 参数合法性判断
	if (pCtrl == NULL)
	{
		printf("para error in ContinuousInterpolation\r\n");
		rslt = -1;		// 输入参数错误
		return rslt;
	}

	if (pCtrl->vAxisId != VAXIS_ID1)
	{
		printf("para vAxisId error in ContinuousInterpolation\r\n");
		rslt = -1;		// 输入参数错误
		return rslt;		
	}

	for (i = 0; i < AXIS_NUM; i++)	// 统一实轴速度来源
	{
		pCtrl->motosConfig.axisConfig[i].spdSource = MAKE_POUT_CMD(pCtrl->vAxisId);
	}

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

	if (pCtrl->GetEmergencyStop != NULL)	// 紧急停止条件函数
	{
		err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->mvData.errInfo = err;
			printf("emergency stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
			rslt = 2;		// 紧急停止条件触发停车，立刻停止脉冲发送
			return rslt;
		}
	}

	if (pCtrl->GetQuickStop != NULL)		// 出错停止条件函数
	{
		err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->mvData.errInfo = err;
			printf("quick stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
			rslt = 3;		// 快速停止条件触发停车
			return rslt;
		}
	}
	
	if (pCtrl->GetNormalStop != NULL)		// 一般停止
	{
		err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);
		if (err != 0)
		{
			pCtrl->mvData.errInfo = err;
			printf("normal stop 1 in ContinuousInterpolation, err=0x%x\r\n", err);
			rslt = 4;		// 普通停车条件触发停车
			return rslt;
		}
	}

	// 选择参与电机
	SetMotosConfig(&(pCtrl->motosConfig));

	// 等待驱动器执行
	if (pCtrl->Delay != NULL && pCtrl->motosConfig.newConfig != 0)
	{
		pCtrl->Delay(100);
	}

	bufsta = 0;
	bufsel = 0;
	decCount = 0;
	pCtrl->mvData.fillLimit = 0;

	decPPSG = MIN_GAP;

	pCtrl->mvData.errInfo = 0;

	vAxisId = pCtrl->vAxisId;

	if (vAxisId == VAXIS_ID1)
	{
		vAxisCleanOv1 = VCTR_VAXIS1_CLEAN_OV1;
		vAxisCleanOv2 = VCTR_VAXIS1_CLEAN_OV2;

		vAxisBuf1Finish = VAXIS1_BUF1_FINISH;
		vAxisBuf2Finish = VAXIS1_BUF2_FINISH;

		vAxisBuf1Running = VAXIS1_BUF1_RUNNING;
		vAxisBuf2Running = VAXIS1_BUF2_RUNNING;

//		vAxisDisBuf1 = VCTR_VAXIS1_DIS_BUF1;
//		vAxisDisBuf2 = VCTR_VAXIS1_DIS_BUF2;
	}
	else
	{
		printf("not support vaxis\r\n");
	}

	if (pCtrl->WorkBeforeRun != NULL)
	{
		pCtrl->WorkBeforeRun(pCtrl->workPara1, pCtrl->workPara2);
	}

	SetVAxisConfig(VCTR_AXIS_ALM_INT);	// 清除报警状态
	memset(&idata, 0, sizeof(InterData));

	do
	{
		pCtrl->mvData.bufsta = bufsta;
		// 刷新数据
		if (pCtrl->RefreshData != NULL)
		{
			rslt = pCtrl->RefreshData(pCtrl->refDatPara, (u32)(&(pCtrl->mvData)));
		}

		// 更新坐标
		if (GetMotosCounter(position) == 0)
		{
			RefreshMotosPos(position);			// 更新各个轴的位置
			if (pCtrl->RefreshPosition != NULL)
			{
				pCtrl->RefreshPosition(pCtrl->refPosPara, position);
			}
		}
		else
		{
			rslt = 5;
			break;
		}

		// 填充数据
		if ((bufsta & 0x03) != 0x03 && pCtrl->mvData.mvmtStatus != 0)		// 当Buff有空闲，并且还有数据时
		{
			if (pCtrl->mvData.runstat != CRST_BREAK || pCtrl->mvData.fillLimit > 0)		// 非刹车过程 或 在刹车过程中且允许填充数据
			{
				if (pCtrl->mvData.fillLimit > 0)
				{
					pCtrl->mvData.fillLimit--;
				}

				// 写各个轴的插补数据
				for (i = 0; i < AXIS_NUM; i++)
				{
					if (pCtrl->mvData.mvmtdat.segmnetNum[i] != 0)	// 有效段数
					{
						idata.datBufLen = pCtrl->mvData.mvmtdat.segmnetNum[i];		// 有效段个数
						// 计算该段插补需要的实轴数据
						//--------------------------
						idata.axisId = g_axisIDList[i];		// 实轴选择
						idata.vaxisId = vAxisId;			// 使用虚轴
						if (bufsel == 0)					// 缓冲区选择
						{
							idata.bufSel = BUF_SEL1;		// 缓冲区1
						}
						else
						{
							idata.bufSel = BUF_SEL2;		// 缓冲区2
						}
						idata.startEscNum = pCtrl->mvData.mvmtdat.startEsc[i];		// 起始跳过脉冲数
						idata.partVxNum = pCtrl->mvData.mvmtdat.vppSegment[i];		// 每段对应的虚轴脉冲数
						//---------------- 20220531 修改每段脉冲数限制
						if ((idata.partVxNum * SUPPORT_SEGMENT) < pCtrl->mvData.mvmtdat.interLong)
						{
							idata.partVxNum = (pCtrl->mvData.mvmtdat.interLong+SUPPORT_SEGMENT-1) / SUPPORT_SEGMENT;
						}
						//----------------- 20220531 修改每段脉冲数限制
						idata.dirAttr = AXIS_DIR_USEBUF;							// 使用段内方向

						// 填充多段数据
						for (j = 0; j < pCtrl->mvData.mvmtdat.segmnetNum[i]; j++)
						{
							idata.datBuff[j] = pCtrl->mvData.mvmtdat.movement[i][j];		// 数据段
						}

						idata.datBuff[j] = 0;
						idata.datBufLen += 1;
						
						idata.sineOut = pCtrl->mvData.mvmtdat.sineOut[i];			// 正弦输出模式

						//printf("HEX:%d.datBuff[0]=0x%x\r\n", i+1, idata.datBuff[0]);
						//printf("DEC:%d.datBuff[0]=%d\r\n", i+1, idata.datBuff[0]);

						rslt = WriteInterData(&idata);
						if (rslt < 0)
						{
							return rslt;
						}
						//printf("axis %d wt, c=%d\r\n", i+1, idata.datBuff[0]);
					}
					else
					{
						if (bufsel == 0)
						{
							CleanAxisBuff(g_axisIDList[i], BUF_SEL1);
						}
						else
						{
							CleanAxisBuff(g_axisIDList[i], BUF_SEL2);
						}
					}
				}

				// 启动运动, 虚轴命令
				vAxisCmd.vaxisId = vAxisId;		// 使用虚轴
				if (bufsel == 0)
				{
					bufsta |= 0x01;
					vAxisCmd.bufSel = BUF_SEL1;
//					curtab1num = pCtrl->mvData.interLong;
//					printf("write buff1\r\n");
				}
				else
				{
					bufsta |= 0x02;
					vAxisCmd.bufSel = BUF_SEL2;
//					curtab2num = pCtrl->mvData.interLong;
//					printf("write buff2\r\n");
				}

			//	printf("write add vtable\r\n");
				vAxisCmd.startPPS = pCtrl->mvData.lowPPS;			// 启动速度
				vAxisCmd.runPPS = pCtrl->mvData.runPPS;				// 运行速度
				vAxisCmd.jumpNum = 0;								// 跳过脉冲数
				vAxisCmd.calcGap = pCtrl->mvData.calcGap;			// 计算时间
				vAxisCmd.outNum = pCtrl->mvData.mvmtdat.interLong;	// 插补轴的脉冲个数
				rslt = StartVAxisRun(&vAxisCmd);							// 写运行命令
				if (rslt < 0)
				{
					break;
				}
				bufsel = ~bufsel;					// 切换buf
				pCtrl->mvData.fillCount++;			// 填充数据计数增加
//				pCtrl->mvData.runIdx++;				// 运行计数器
				pCtrl->mvData.mvmtStatus = 0;		// 设置数据执行标志
			}
		}

		// 等待运动结束
		rslt = 0;
		err = 0;

		{
			// 得到虚轴运行状态
			// 为了得到一个正确的运行状态
			tmp16 = GetRunStatus();
			vrsta = GetRunStatus();
			while(tmp16 != vrsta)
			{
//				printf("5. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
				tmp16 = vrsta;
				vrsta = GetRunStatus();
			}
			tmp16 = GetRunStatus();
			vrsta = GetRunStatus();
			while(tmp16 != vrsta)
			{
//				printf("6. run sta not same, old=0x%x, new=0x%x\r\n", tmp16, vrsta);
				tmp16 = vrsta;
				vrsta = GetRunStatus();
			}

			if ((vrsta & (vAxisBuf1Finish|vAxisBuf2Finish)) != 0)		// 虚轴缓冲区1或2完成
			{
				if (pCtrl->mvData.runstat != CRST_NORMAL)		// 处于进入停止状态或刹车状态时，执行完一个表后，重新计算加速度
				{
					s32 inmvmt;
					// 调整减速速度
					u32 curpps = GetVAxisPPS(vAxisId);		// 当前速度

					inmvmt = decCount;
				//	inmvmt -= 1;
					if (inmvmt > 0)
					{
						inmvmt *= pCtrl->mvData.mvmtdat.interLong;

						// 重新计算加速度
						decPPSG = CalcGapTime(inmvmt, curpps, pCtrl->mvData.lowPPS);

					//	printf("pCtrl->mvData.runstat=%d, curpps=%d, mvmt=%d, lopps=%d, recalc decPPSG=%d\r\n", pCtrl->mvData.runstat, curpps, inmvmt, pCtrl->mvData.lowPPS, decPPSG);

						vAxisCmd.vaxisId = vAxisId;		// 使用虚轴
						vAxisCmd.bufSel = BUF_SEL3;
						vAxisCmd.calcGap = decPPSG;
						vAxisCmd.runPPS = pCtrl->mvData.lowPPS;
						vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
						rslt = StartVAxisRun(&vAxisCmd);			// 减速
						if (rslt < 0)
						{
							break;
						}
					}
				}

				if ((vrsta & vAxisBuf1Finish) != 0)		// 虚轴缓冲区1完成标志
				{
					SetVAxisConfig(vAxisCleanOv1);			// 清除表1完成标志
					bufsta &= 0xFE;		// 释放buf1
					pCtrl->mvData.execCount++;				// 已执行数据增加
					pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);

					pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount;		// 正在执行数据的索引（另外一个表的数据） = 数据起始+跳过数据+已经执行数据
			//		printf("table 1 ok, datidx=%d, endpps=%d\r\n", iddx, pCtrl->mvData.lastEndPPS);
			//		printf("table 1 ok, exeingidx=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, pCtrl->mvData.lastEndPPS);

					if (pCtrl->mvData.runstat == CRST_BREAK)		// 进入减速后，第一次切换表开始减速
					{
						if (decCount != 0)
						{
							decCount--;		// 减速执行圈数减少
						}
					//	printf("in break, table 1 ok, exeingIdx=%d, decCount=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, decCount, pCtrl->mvData.lastEndPPS);
					}
				}

				if ((vrsta & vAxisBuf2Finish) != 0)		// 虚轴缓冲区2完成标志
				{
					SetVAxisConfig(vAxisCleanOv2);		// 清除表2完成标志
					bufsta &= 0xFD;		// 释放buf2
					pCtrl->mvData.execCount++;			// 已执行数据增加
					pCtrl->mvData.lastEndPPS = GetLastVAxisFrequency(vAxisId);

					pCtrl->mvData.exeingIdx = pCtrl->dataBegIndex+pCtrl->mvData.emptyCount+pCtrl->mvData.execCount;		// 正在执行数据的索引（另外一个表的数据） = 数据起始+跳过数据+已经执行数据
				//	printf("table 2 ok, datidx=%d, endpps=%d\r\n", iddx, pCtrl->mvData.lastEndPPS);
				//	printf("table 2 ok, exeingidx=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, pCtrl->mvData.lastEndPPS);

					if (pCtrl->mvData.runstat == CRST_BREAK)		// 进入减速后，第一次切换表开始减速
					{
						if (decCount != 0)
						{
							decCount--;		// 减速执行圈数减少
						}
					//	printf("in break, table 2 ok, exeingIdx=%d, decCount=%d, endpps=%d\r\n", pCtrl->mvData.exeingIdx, decCount, pCtrl->mvData.lastEndPPS);
					}
				}
			}

			if ((vrsta & AXIS_ALARM_FLAG) != 0)		// 实轴报警标志
			{
				err = GetAlarmValue();				// 得到轴报警信息
				SetVAxisConfig(VCTR_AXIS_ALM_INT);	// 清除状态
				if (err != 0)
				{
					printf("2. axis alarm value=0x%x\r\n", err);
				}
				rslt = 1;		// 驱动器报警停车
				break;
			}
			else if ((vrsta & (vAxisBuf1Running | vAxisBuf2Running)) == 0)	// 两个缓冲区的数据都执行完成，没有有效数据
			{
				if (pCtrl->mvData.waitNoData == 0)
				{
					rslt = 0;							// 无有效数据，结束
//					printf("no more data in two buff in ContinuousInterpolation, err=0x%x\r\n", pCtrl->mvData.errInfo);
					break;
				}
				else
				{
//					printf("wait when no data\r\n");
				}
			}
		}

		if (pCtrl->GetEmergencyStop != NULL)	// 急停条件函数
		{
			err = pCtrl->GetEmergencyStop(pCtrl->condPara1, pCtrl->condPara2);
			if (err != 0)
			{
				pCtrl->mvData.errInfo = err;
				printf("2. emergency stop 3, err=0x%x\r\n", err);
				rslt = 2;		// 紧急停止条件触发停车，立刻停止脉冲发送
				break;
			}
		}

		if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetQuickStop != NULL)		// 快速停止条件函数
		{
			err = pCtrl->GetQuickStop(pCtrl->condPara1, pCtrl->condPara2);
			if (err != 0)
			{
			//	printf("quick stop, enter break status, rslt=0x%x\r\n", err);
				pCtrl->mvData.errInfo = err;

				// 设置当前速度为运行速度，保持速度不变
				decPPSG = pCtrl->qstopPPSG;

				vAxisCmd.vaxisId = vAxisId;		// 使用虚轴
				vAxisCmd.bufSel = BUF_SEL3;
				vAxisCmd.calcGap = decPPSG;
				vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
				vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
				rslt = StartVAxisRun(&vAxisCmd);
				if (rslt < 0)
				{
					break;
				}
				// 计算以当前运行速度减速到停止速度需要的位移
				decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);
				if (pCtrl->mvData.mvmtdat.interLong != 0)
				{
					decCount += pCtrl->mvData.mvmtdat.interLong-1;
					decCount /= pCtrl->mvData.mvmtdat.interLong;					// 计算得到减速需要的整圈数
				}
				else
				{
					printf("1. Err: interLong==0\r\n");
				}

				//printf("1. runpps=%d, lowPPS=%d, calcGap=%d, decCount=%d\r\n", vAxisCmd.runPPS, vAxisCmd.startPPS, vAxisCmd.calcGap, decCount);

				pCtrl->mvData.fillLimit = decCount+pCtrl->mvData.execCount-pCtrl->mvData.fillCount;	// 需要继续填充的数据计数

				/* 20220406 ljs 填充限制+1
				 * 降速所需圈数decCount,求出的这个值是正确的
				 * pCtrl->mvData.execCount是执行完成计数值
				 * pCtrl->mvData.fillCount是填充计数值
				 * 代码执行到此处时,一定有一个表正在执行,且另外一个表已经被填充
				 * 正在执行的表是不能降速的,只有未执行的表是可以降速的
				 * 故上述填充限制计算得出的值会少填充一针数据
				 * 例如,此时正在执行第113针,已经填充了第114针,那么第113针不考虑降速,从第114针开始降速
				 * 若填充计数值为65,此时执行完成计数值为63(因为第113针尚未执行完成,故差值为2)
				 * 若求出需要4圈降速,那么第114,115,116,117针需要降速,且可以降速到目标速度
				 * 根据上述计算方式求出的填充限制值为2,则可继续填充执行第115,116针,则第117针虽然可以填充,
				 * 但因为此时填充限制值为0,故虽然填充了,但不执行
				 * 所以实际只有3针用于降速
				 * 这就是为什么降速不到目标速度的原因
				 */
				pCtrl->mvData.fillLimit += 1;

				//printf("1. fill limit=%d, decCount=%d,fillCount=%d,exeCount=%d\r\n", pCtrl->mvData.fillLimit, decCount,pCtrl->mvData.fillCount,pCtrl->mvData.execCount);
				if (pCtrl->mvData.fillLimit < 0)
				{
			//		printf("1. no need to fill data\r\n");
					pCtrl->mvData.fillLimit = 0;
				}
				pCtrl->mvData.runstat = CRST_QSTOP;			// 设置停车状态
			}
		}

		if (pCtrl->mvData.runstat == CRST_NORMAL && pCtrl->GetNormalStop != NULL)		// 一般停止
		{
			err = pCtrl->GetNormalStop(pCtrl->condPara1, pCtrl->condPara2);

			if (err != 0)
			{
			//	printf("normal stop, enter break status, rslt=0x%x\r\n", err);
				pCtrl->mvData.errInfo = err;

				// 设置当前速度为运行速度，保持速度不变
				decPPSG = pCtrl->nstopPPSG;

				vAxisCmd.vaxisId = vAxisId;		// 使用虚轴
				vAxisCmd.bufSel = BUF_SEL3;
				vAxisCmd.calcGap = decPPSG;
				vAxisCmd.runPPS = GetVAxisPPS(vAxisId);
				vAxisCmd.startPPS = pCtrl->mvData.lowPPS;
				rslt = StartVAxisRun(&vAxisCmd);
				if (rslt < 0)
				{
					break;
				}
				// 计算以当前运行速度减速到停止速度需要的位移
				decCount = CalcDisplacement(vAxisCmd.runPPS, vAxisCmd.startPPS, decPPSG);

				if (pCtrl->mvData.mvmtdat.interLong != 0)
				{
					decCount += pCtrl->mvData.mvmtdat.interLong-1;
					decCount /= pCtrl->mvData.mvmtdat.interLong;					// 计算得到减速需要的整圈数
				}
				else
				{
					printf("2. Err: interLong==0\r\n");
				}
				//printf("2. runpps=%d, lowPPS=%d, calcGap=%d, decCount=%d\r\n", vAxisCmd.runPPS, vAxisCmd.startPPS, vAxisCmd.calcGap, decCount);

				pCtrl->mvData.fillLimit = decCount+pCtrl->mvData.execCount-pCtrl->mvData.fillCount;	// 需要继续填充的数据计数

				// 20220406 ljs 填充限制+1
				pCtrl->mvData.fillLimit += 1;

				//printf("2. fill limit=%d, decCount=%d,fillCount=%d,exeCount=%d\r\n", pCtrl->mvData.fillLimit, decCount,pCtrl->mvData.fillCount,pCtrl->mvData.execCount);
				if (pCtrl->mvData.fillLimit < 0)
				{
			//		printf("1. no need to fill data\r\n");
					pCtrl->mvData.fillLimit = 0;
				}
				pCtrl->mvData.runstat = CRST_NSTOP;			// 设置停车状态
			}
		}

	}while(1);

	StopVAxisRun(vAxisId);

	if (pCtrl->WorkAfterRun != NULL)
	{
		pCtrl->WorkAfterRun(pCtrl->workPara1, pCtrl->workPara2);
	}


	// 更新坐标
	if (GetMotosCounter(position) == 0)
	{
		RefreshMotosPos(position);			// 更新各个轴的位置
		if (pCtrl->RefreshPosition != NULL)
		{
			pCtrl->RefreshPosition(pCtrl->refPosPara, position);
		}
	}
	else
	{
		rslt = 5;
	}

	return rslt;
}

#endif