Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/hal/lib/athena/ctre/CtreCanNode.cpp b/hal/lib/athena/ctre/CtreCanNode.cpp
new file mode 100644
index 0000000..2eb42ab
--- /dev/null
+++ b/hal/lib/athena/ctre/CtreCanNode.cpp
@@ -0,0 +1,163 @@
+#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
+
+#include "ctre/CtreCanNode.h"
+#include "FRC_NetworkCommunication/CANSessionMux.h"
+#include <string.h> // memset
+
+static const UINT32 kFullMessageIDMask = 0x1fffffff;
+
+CtreCanNode::CtreCanNode(UINT8 deviceNumber)
+{
+ _deviceNumber = deviceNumber;
+}
+CtreCanNode::~CtreCanNode()
+{
+}
+void CtreCanNode::RegisterRx(uint32_t arbId)
+{
+ /* no need to do anything, we just use new API to poll last received message */
+}
+/**
+ * Schedule a CAN Frame for periodic transmit.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @param dlc Number of bytes to transmit (0 to 8).
+ * @param initialFrame Ptr to the frame data to schedule for transmitting. Passing null will result
+ * in defaulting to zero data value.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame)
+{
+ int32_t status = 0;
+ if(dlc > 8)
+ dlc = 8;
+ txJob_t job = {0};
+ job.arbId = arbId;
+ job.periodMs = periodMs;
+ job.dlc = dlc;
+ if(initialFrame){
+ /* caller wants to specify original data */
+ memcpy(job.toSend, initialFrame, dlc);
+ }
+ _txJobs[arbId] = job;
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( job.arbId,
+ job.toSend,
+ job.dlc,
+ job.periodMs,
+ &status);
+}
+/**
+ * Schedule a CAN Frame for periodic transmit. Assume eight byte DLC and zero value for initial transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ */
+void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs)
+{
+ RegisterTx(arbId,periodMs, 8, 0);
+}
+/**
+ * Remove a CAN frame Arbid to stop transmission.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ */
+void CtreCanNode::UnregisterTx(uint32_t arbId)
+{
+ /* set period to zero */
+ ChangeTxPeriod(arbId, 0);
+ /* look and remove */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ _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;
+}
+CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
+{
+ CTR_Code retval = CTR_OKAY;
+ int32_t status = 0;
+ uint8_t len = 0;
+ uint32_t timeStamp;
+ /* cap timeout at 999ms */
+ if(timeoutMs > 999)
+ timeoutMs = 999;
+ FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+ 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 */
+ memcpy(r.bytes, dataBytes, 8); /* fill in databytes */
+ }else{
+ /* did not get the message */
+ rxRxEvents_t::iterator i = _rxRxEvents.find(arbId);
+ if(i == _rxRxEvents.end()){
+ /* we've never gotten this mesage */
+ retval = CTR_RxTimeout;
+ /* fill caller's buffer with zeros */
+ memset(dataBytes,0,8);
+ }else{
+ /* 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 */
+ /* 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 */
+ }
+ }
+ }
+
+ return retval;
+}
+void CtreCanNode::FlushTx(uint32_t arbId)
+{
+ int32_t status = 0;
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end())
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+}
+/**
+ * Change the transmit period of an already scheduled CAN frame.
+ * This keeps the frame payload contents the same without caller having to perform
+ * a read-modify-write.
+ * @param arbId CAN Frame Arbitration ID. Set BIT31 for 11bit ids, otherwise we use 29bit ids.
+ * @param periodMs Period to transmit CAN frame. Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
+ * @return true if scheduled job was found and updated, false if there was no preceding job for the specified arbID.
+ */
+bool CtreCanNode::ChangeTxPeriod(uint32_t arbId, uint32_t periodMs)
+{
+ int32_t status = 0;
+ /* lookup the data bytes and period for this message */
+ txJobs_t::iterator iter = _txJobs.find(arbId);
+ if(iter != _txJobs.end()) {
+ /* modify th periodMs */
+ iter->second.periodMs = periodMs;
+ /* reinsert into scheduler with the same data bytes, only the period changed. */
+ FRC_NetworkCommunication_CANSessionMux_sendMessage( iter->second.arbId,
+ iter->second.toSend,
+ iter->second.dlc,
+ iter->second.periodMs,
+ &status);
+ return true;
+ }
+ return false;
+}
+