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; }