STATUS clockLibInit(
void
)
{
STATUS status;
if (clockLibInstalled == TRUE)
{
status = OK;
}
else
{
if (sysClockRateGet() < 1)
{
status = ERROR;
}
memset(&_clockRealtime, 0, sizeof(CLOCK));
_clockRealtime.tickBase = tick64Get();
_clockRealtime.timeBase.tv_sec = 0;
_clockRealtime.timeBase.tv_nsec = 0;
clockLibInstalled = TRUE;
status = OK;
}
return status;
}
int clock_settime(
clockid_t id,
const struct timespec *tp
)
{
int ret;
if (id != CLOCK_REALTIME)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
if ((tp == NULL) || (tp->tv_sec < 0) || (tp->tv_nsec >= BILLION))
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
_clockRealtime.tickBase = tick64Get();
_clockRealtime.timeBase = *tp;
ret = OK;
}
}
return ret;
}
int clock_gettime(
clockid_t id,
struct timespec *tp
)
{
int ret;
u_int64_t diff;
if (id != CLOCK_REALTIME)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
if (tp == NULL)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
diff = tick64Get() - _clockRealtime.tickBase;
TV_CONVERT_TO_SEC(*tp, diff);
TV_ADD(*tp, _clockRealtime.timeBase);
ret = OK;
}
}
return ret;
}
extern "C" int gettimeofday(struct timeval *tp, void *tzp)
{
static int clkRate = 0;
unsigned long long ticks;
if ( clkRate == 0 )
clkRate = sysClkRateGet(); /* expensive call, via sysctl in RTP */
ticks = tick64Get(); /* also via sysctl, but unavoidable */
/* The tv_sec member of struct timeval is presently of type 'long'.
* The tv_sec member of struct timespec is time_t,
* which is unsigned long. * We don't want to return a
* negative result for tv_sec.
*/
tp->tv_sec = ( long )( ticks / clkRate ) & LONG_MAX;
tp->tv_usec = ( long )( ticks % clkRate ) * 1000000 / clkRate;
return 0;
}
STATUS runPreset()
{
UINT64 endTick = 0;
UINT64 eStartTick = 0;
if(GV::isStopAll)
{
DISPLAY_FAULT("系统已清除状态,停止接收启动指令,启动失败!");
file.writeLogFile("系统已清除状态,停止接收启动指令!");
return ERROR;
}
for(UINT nodeNum = 0;nodeNum < AXIS_NUM;nodeNum++){
if(!GV::isPowerOutput[nodeNum] || !GV::isZeroPosSet[nodeNum] || (GV::opMode[nodeNum] != 0x1) || GV::isBoltOn[nodeNum] || GV::isLimit[nodeNum]){
printf("%d %d %d %x\n",GV::isPowerOutput[nodeNum],GV::isZeroPosSet[nodeNum],GV::isBoltOn[nodeNum],GV::opMode[nodeNum]);
DISPLAY_FAULT("启动失败,请检查驱动器是否上电,转台是否处在插销和限位状态!");
return ERROR;}}
CAN_PACKET sendPack,recvPack;
stringstream ss;
memset(&sendPack,0,CP_SIZE);
memset(&recvPack,0,CP_SIZE);
UCHAR buf[WCTLMSG_SIZE];
memset(buf, 0, WCTLMSG_SIZE);
memcpy(buf, &GV::ctrlParamMsg, WCTLMSG_SIZE);
int validBit = 0;
for(validBit = 0;validBit < 5;validBit++){
if(buf[2 + validBit] ^ 0xff){
break;}}
if(buf[2 + validBit] == 0xa0){
control.findZeroPos();
return OK;}
else{
ss << buf[2 + validBit] - 0xa0;
for(int seg = 0; seg < file.jsonDoc[ss.str().c_str()]["seg"].GetInt(); seg++)
{
if(OK != semTake(GV::isCANOccupy,SEM_TIME_OUT*SYS_FREQ))
{
DISPLAY_FAULT("启动操作:获取CAN总线资源失败!");
file.writeLogFile("启动操作:获取CAN锁信号量超时!");
return ERROR;
}
can.writeCopleyCMD(sendPack,
DRIVER_CMD_POS_SET,
file.jsonDoc[ss.str().c_str()]["angle"][seg].GetDouble(),
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
can.writeCopleyCMD(sendPack,
DRIVER_CMD_VEL_SET,
fabs(file.jsonDoc[ss.str().c_str()]["angle"][seg].GetDouble() / file.jsonDoc[ss.str().c_str()]["time"][seg].GetDouble()) * 1.2,
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
can.writeCopleyCMD(sendPack,
DRIVER_CMD_ACC_SET,
MAX_ACC[validBit],
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
can.writeCopleyCMD(sendPack,
DRIVER_CMD_DEACC_SET,
MAX_ACC[validBit],
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
can.writeCopleyCMD(sendPack,
DRIVER_CLEAR_MOVE,
0,
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
can.writeCopleyCMD(sendPack,
DRIVER_REL_START,
0,
validBit + 1);
if(ERROR == can.uCMDSend(sendPack,recvPack,USING_CAN_CHANNEL)){
semGive(GV::isCANOccupy);
return ERROR;}
semGive(GV::isCANOccupy);
endTick = tick64Get();
taskDelay(static_cast<int>((file.jsonDoc[ss.str().c_str()]["time"][seg].GetDouble()
* SYS_FREQ)) - (seg == 0 ? 0 : (endTick - eStartTick)));
eStartTick = tick64Get();
}}
return OK;
}
