1417 lines
27 KiB
C
1417 lines
27 KiB
C
|
||
#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);
|
||
}
|
||
}
|
||
|
||
|