diff --git a/hal/src/main/native/athena/ctre/CtreCanNode.cpp b/hal/src/main/native/athena/ctre/CtreCanNode.cpp index 8772a4715e..250088735c 100644 --- a/hal/src/main/native/athena/ctre/CtreCanNode.cpp +++ b/hal/src/main/native/athena/ctre/CtreCanNode.cpp @@ -68,17 +68,13 @@ void CtreCanNode::UnregisterTx(uint32_t arbId) _txJobs.erase(iter); } } -timespec diff(const timespec & start, const timespec & end) -{ - timespec temp; - if ((end.tv_nsec-start.tv_nsec)<0) { - temp.tv_sec = end.tv_sec-start.tv_sec-1; - temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec; - } else { - temp.tv_sec = end.tv_sec-start.tv_sec; - temp.tv_nsec = end.tv_nsec-start.tv_nsec; - } - return temp; +static int64_t GetTimeMs() { + std::chrono::time_point < std::chrono::system_clock > now; + now = std::chrono::system_clock::now(); + auto duration = now.time_since_epoch(); + auto millis = std::chrono::duration_cast < std::chrono::milliseconds + > (duration).count(); + return (int64_t) millis; } CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs) { @@ -90,10 +86,11 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout if(timeoutMs > 999) timeoutMs = 999; FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status); + std::lock_guard lock(_lck); if(status == 0){ /* fresh update */ rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */ - clock_gettime(2,&r.time); /* fill in time */ + r.time = GetTimeMs(); memcpy(r.bytes, dataBytes, 8); /* fill in databytes */ }else{ /* did not get the message */ @@ -107,16 +104,13 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout /* we've gotten this message before but not recently */ memcpy(dataBytes,i->second.bytes,8); /* get the time now */ - struct timespec temp; - clock_gettime(2,&temp); /* get now */ + int64_t now = GetTimeMs(); /* get now */ /* how long has it been? */ - temp = diff(i->second.time,temp); /* temp = now - last */ - if(temp.tv_sec > 0){ - retval = CTR_RxTimeout; - }else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){ - retval = CTR_RxTimeout; - }else { - /* our last update was recent enough */ + int64_t temp = now - i->second.time; /* temp = now - last */ + if (temp > ((int64_t) timeoutMs)) { + retval = CTR_RxTimeout; + } else { + /* our last update was recent enough */ } } } diff --git a/hal/src/main/native/athena/ctre/CtreCanNode.h b/hal/src/main/native/athena/ctre/CtreCanNode.h index 270759866c..c090cc39c7 100644 --- a/hal/src/main/native/athena/ctre/CtreCanNode.h +++ b/hal/src/main/native/athena/ctre/CtreCanNode.h @@ -4,6 +4,7 @@ #include #include // memcpy #include +#include class CtreCanNode { public: @@ -108,7 +109,7 @@ private: class rxEvent_t{ public: uint8_t bytes[8]; - struct timespec time; + int64_t time; rxEvent_t() { bytes[0] = 0; @@ -127,5 +128,7 @@ private: typedef std::map rxRxEvents_t; rxRxEvents_t _rxRxEvents; + + wpi::mutex _lck; }; #endif