diff --git a/hal/src/main/java/edu/wpi/first/wpilibj/CANData.java b/hal/src/main/java/edu/wpi/first/wpilibj/CANData.java new file mode 100644 index 0000000000..7e0fc6e35b --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/wpilibj/CANData.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +public class CANData { + @SuppressWarnings("MemberName") + public final byte[] data = new byte[8]; + @SuppressWarnings("MemberName") + public int length; + @SuppressWarnings("MemberName") + public long timestamp; + + /** + * API used from JNI to set the data. + */ + public byte[] setData(int length, long timestamp) { + this.length = length; + this.timestamp = timestamp; + return data; + } +} diff --git a/hal/src/main/java/edu/wpi/first/wpilibj/hal/CANAPIJNI.java b/hal/src/main/java/edu/wpi/first/wpilibj/hal/CANAPIJNI.java new file mode 100644 index 0000000000..9085653c09 --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/wpilibj/hal/CANAPIJNI.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.hal; + +import edu.wpi.first.wpilibj.CANData; + +@SuppressWarnings("AbbreviationAsWordInName") +public class CANAPIJNI extends JNIWrapper { + public static native int initializeCAN(int manufacturer, int deviceId, int deviceType); + + public static native void cleanCAN(int handle); + + public static native void writeCANPacket(int handle, byte[] data, int apiId); + + public static native void writeCANPacketRepeating(int handle, byte[] data, int apiId, + int repeatMs); + + public static native void stopCANPacketRepeating(int handle, int apiId); + + public static native boolean readCANPacketNew(int handle, int apiId, CANData data); + + public static native boolean readCANPacketLatest(int handle, int apiId, CANData data); + + public static native boolean readCANPacketTimeout(int handle, int apiId, int timeoutMs, + CANData data); + + public static native boolean readCANPeriodicPacket(int handle, int apiId, int timeoutMs, + int periodMs, CANData data); +} diff --git a/hal/src/main/native/athena/CANAPI.cpp b/hal/src/main/native/athena/CANAPI.cpp new file mode 100644 index 0000000000..6a77ca9994 --- /dev/null +++ b/hal/src/main/native/athena/CANAPI.cpp @@ -0,0 +1,347 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/CANAPI.h" + +#include +#include + +#include + +#include "HAL/CAN.h" +#include "HAL/Errors.h" +#include "HAL/HAL.h" +#include "HAL/handles/UnlimitedHandleResource.h" + +using namespace hal; + +namespace { +struct Receives { + uint64_t lastTimeStamp; + uint8_t data[8]; + uint8_t length; +}; + +struct CANStorage { + HAL_CANManufacturer manufacturer; + HAL_CANDeviceType deviceType; + uint8_t deviceId; + wpi::mutex mapMutex; + wpi::SmallDenseMap periodicSends; + wpi::SmallDenseMap receives; +}; +} // namespace + +static UnlimitedHandleResource* + canHandles; + +static std::atomic_bool HasFixedTime{false}; +static uint64_t timeSpanDiff; + +static void CheckDeltaTime() { + if (HasFixedTime) return; + HasFixedTime = true; + + // TODO: Fix locking + timespec t; + clock_gettime(CLOCK_MONOTONIC, &t); + + int32_t status = 0; + uint64_t fpgaTime = HAL_GetFPGATime(&status); + + // Convert t to microseconds + uint64_t us = t.tv_sec * 1000000 + t.tv_nsec / 1000; + + timeSpanDiff = + us - fpgaTime; // This assumes CLOCK_MONOTONIC is greater then FPGA Time. +} + +static inline uint64_t ConvertToFPGATime(uint32_t canMs) { + uint64_t canMsToUs = canMs * 1000; + return canMsToUs - timeSpanDiff; +} + +namespace hal { +namespace init { +void InitializeCANAPI() { + static UnlimitedHandleResource + cH; + canHandles = &cH; +} +} // namespace init +} // namespace hal + +static int32_t CreateCANId(CANStorage* storage, int32_t apiId) { + int32_t createdId = 0; + createdId |= (static_cast(storage->deviceType) & 0x1F) << 24; + createdId |= (static_cast(storage->manufacturer) & 0xFF) << 16; + createdId |= (apiId & 0x3FF) << 6; + createdId |= (storage->deviceId & 0x3F); + return createdId; +} + +HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, + int32_t deviceId, HAL_CANDeviceType deviceType, + int32_t* status) { + CheckDeltaTime(); + auto can = std::make_shared(); + + auto handle = canHandles->Allocate(can); + + if (handle == HAL_kInvalidHandle) { + *status = NO_AVAILABLE_RESOURCES; + return HAL_kInvalidHandle; + } + + can->deviceId = deviceId; + can->deviceType = deviceType; + can->manufacturer = manufacturer; + + return handle; +} + +void HAL_CleanCAN(HAL_CANHandle handle) { + auto data = canHandles->Free(handle); + + std::lock_guard lock(data->mapMutex); + + for (auto&& i : data->periodicSends) { + int32_t s = 0; + HAL_CAN_SendMessage(i.first, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, + &s); + i.second = -1; + } +} + +void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = -1; +} + +void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, + int32_t repeatMs, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, data, length, repeatMs, status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = repeatMs; +} + +void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, + status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = -1; +} + +void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + if (*status == 0) { + std::lock_guard lock(can->mapMutex); + auto& msg = can->receives[id]; + msg.length = dataSize; + msg.lastTimeStamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } + *length = dataSize; + *receivedTimestamp = timestamp; +} + +void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + // Found, check if new enough + uint64_t now = HAL_GetFPGATime(status); + if (now - i->second.lastTimeStamp > + static_cast(timeoutMs) * 1000) { + // Timeout, return bad status + *status = HAL_CAN_TIMEOUT; + return; + } + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t periodMs, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + { + std::lock_guard lock(can->mapMutex); + auto i = can->receives.find(id); + if (i != can->receives.end()) { + uint64_t now = HAL_GetFPGATime(status); + // Found, check if new enough + if (now - i->second.lastTimeStamp < + static_cast(periodMs) * 1000) { + *status = 0; + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + } + } + } + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + // Found, check if new enough + uint64_t now = HAL_GetFPGATime(status); + if (now - i->second.lastTimeStamp > + static_cast(timeoutMs) * 1000) { + // Timeout, return bad status + *status = HAL_CAN_TIMEOUT; + return; + } + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 942282c03e..85819d837e 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -49,6 +49,7 @@ void InitializeHAL() { InitializeAnalogOutput(); InitializeAnalogTrigger(); InitializeCAN(); + InitializeCANAPI(); InitializeCompressor(); InitializeConstants(); InitializeCounter(); @@ -207,6 +208,8 @@ const char* HAL_GetErrorMessage(int32_t code) { return HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE; case HAL_SERIAL_PORT_ERROR: return HAL_SERIAL_PORT_ERROR_MESSAGE; + case HAL_CAN_TIMEOUT: + return HAL_CAN_TIMEOUT_MESSAGE; default: return "Unknown error status"; } diff --git a/hal/src/main/native/athena/HALInitializer.h b/hal/src/main/native/athena/HALInitializer.h index dfb4c70847..32feff4e86 100644 --- a/hal/src/main/native/athena/HALInitializer.h +++ b/hal/src/main/native/athena/HALInitializer.h @@ -26,6 +26,7 @@ extern void InitializeAnalogInternal(); extern void InitializeAnalogOutput(); extern void InitializeAnalogTrigger(); extern void InitializeCAN(); +extern void InitializeCANAPI(); extern void InitializeCompressor(); extern void InitializeConstants(); extern void InitializeCounter(); diff --git a/hal/src/main/native/cpp/jni/CANAPIJNI.cpp b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp new file mode 100644 index 0000000000..1ff21ea0f8 --- /dev/null +++ b/hal/src/main/native/cpp/jni/CANAPIJNI.cpp @@ -0,0 +1,241 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include +#include +#include + +#include "HAL/CAN.h" +#include "HAL/CANAPI.h" +#include "HAL/Errors.h" +#include "HAL/cpp/Log.h" +#include "HALUtil.h" +#include "edu_wpi_first_wpilibj_hal_CANAPIJNI.h" + +using namespace frc; +using namespace wpi::java; + +extern "C" { +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: initializeCAN + * Signature: (III)I + */ +JNIEXPORT jint JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_initializeCAN + (JNIEnv* env, jclass, jint manufacturer, jint deviceId, jint deviceType) +{ + int32_t status = 0; + auto handle = + HAL_InitializeCAN(static_cast(manufacturer), + static_cast(deviceId), + static_cast(deviceType), &status); + + CheckStatusForceThrow(env, status); + return handle; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: cleanCAN + * Signature: (I)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_cleanCAN + (JNIEnv* env, jclass, jint handle) +{ + HAL_CleanCAN(static_cast(handle)); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: writeCANPacket + * Signature: (I[BI)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_writeCANPacket + (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId) +{ + auto halHandle = static_cast(handle); + JByteArrayRef arr{env, data}; + auto arrRef = arr.array(); + int32_t status = 0; + HAL_WriteCANPacket(halHandle, reinterpret_cast(arrRef.data()), + arrRef.size(), apiId, &status); + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: writeCANPacketRepeating + * Signature: (I[BII)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_writeCANPacketRepeating + (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId, + jint timeoutMs) +{ + auto halHandle = static_cast(handle); + JByteArrayRef arr{env, data}; + auto arrRef = arr.array(); + int32_t status = 0; + HAL_WriteCANPacketRepeating(halHandle, + reinterpret_cast(arrRef.data()), + arrRef.size(), apiId, timeoutMs, &status); + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: stopCANPacketRepeating + * Signature: (II)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_stopCANPacketRepeating + (JNIEnv* env, jclass, jint handle, jint apiId) +{ + auto halHandle = static_cast(handle); + int32_t status = 0; + HAL_StopCANPacketRepeating(halHandle, apiId, &status); + CheckStatus(env, status); +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: readCANPacketNew + * Signature: (IILjava/lang/Object;)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketNew + (JNIEnv* env, jclass, jint handle, jint apiId, jobject data) +{ + auto halHandle = static_cast(handle); + uint8_t dataTemp[8]; + int32_t dataLength = 0; + uint64_t timestamp = 0; + int32_t status = 0; + HAL_ReadCANPacketNew(halHandle, apiId, dataTemp, &dataLength, ×tamp, + &status); + if (status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (!CheckStatus(env, status)) { + return false; + } + if (dataLength > 8) dataLength = 8; + + jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp); + auto javaLen = env->GetArrayLength(toSetArray); + if (javaLen < dataLength) dataLength = javaLen; + env->SetByteArrayRegion(toSetArray, 0, dataLength, + reinterpret_cast(dataTemp)); + return true; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: readCANPacketLatest + * Signature: (IILjava/lang/Object;)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketLatest + (JNIEnv* env, jclass, jint handle, jint apiId, jobject data) +{ + auto halHandle = static_cast(handle); + uint8_t dataTemp[8]; + int32_t dataLength = 0; + uint64_t timestamp = 0; + int32_t status = 0; + HAL_ReadCANPacketLatest(halHandle, apiId, dataTemp, &dataLength, ×tamp, + &status); + if (status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (!CheckStatus(env, status)) { + return false; + } + if (dataLength > 8) dataLength = 8; + + jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp); + auto javaLen = env->GetArrayLength(toSetArray); + if (javaLen < dataLength) dataLength = javaLen; + env->SetByteArrayRegion(toSetArray, 0, dataLength, + reinterpret_cast(dataTemp)); + return true; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: readCANPacketTimeout + * Signature: (IIILjava/lang/Object;)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketTimeout + (JNIEnv* env, jclass, jint handle, jint apiId, jint timeoutMs, jobject data) +{ + auto halHandle = static_cast(handle); + uint8_t dataTemp[8]; + int32_t dataLength = 0; + uint64_t timestamp = 0; + int32_t status = 0; + HAL_ReadCANPacketTimeout(halHandle, apiId, dataTemp, &dataLength, ×tamp, + timeoutMs, &status); + if (status == HAL_CAN_TIMEOUT || + status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (!CheckStatus(env, status)) { + return false; + } + if (dataLength > 8) dataLength = 8; + + jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp); + auto javaLen = env->GetArrayLength(toSetArray); + if (javaLen < dataLength) dataLength = javaLen; + env->SetByteArrayRegion(toSetArray, 0, dataLength, + reinterpret_cast(dataTemp)); + return true; +} + +/* + * Class: edu_wpi_first_wpilibj_hal_CANAPIJNI + * Method: readCANPeriodicPacket + * Signature: (IIIILjava/lang/Object;)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPeriodicPacket + (JNIEnv* env, jclass, jint handle, jint apiId, jint timeoutMs, jint periodMs, + jobject data) +{ + auto halHandle = static_cast(handle); + uint8_t dataTemp[8]; + int32_t dataLength = 0; + uint64_t timestamp = 0; + int32_t status = 0; + HAL_ReadCANPeriodicPacket(halHandle, apiId, dataTemp, &dataLength, ×tamp, + timeoutMs, periodMs, &status); + if (status == HAL_CAN_TIMEOUT || + status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (!CheckStatus(env, status)) { + return false; + } + if (dataLength > 8) dataLength = 8; + + jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp); + auto javaLen = env->GetArrayLength(toSetArray); + if (javaLen < dataLength) dataLength = javaLen; + env->SetByteArrayRegion(toSetArray, 0, dataLength, + reinterpret_cast(dataTemp)); + return true; +} +} // extern "C" diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index 2d17ddc465..5e2470ca50 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -59,6 +59,7 @@ static JClass pwmConfigDataResultCls; static JClass canStatusCls; static JClass matchInfoDataCls; static JClass accumulatorResultCls; +static JClass canDataCls; namespace frc { @@ -237,6 +238,15 @@ void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult, env->CallVoidMethod(accumulatorResult, func, (jlong)value, (jlong)count); } +jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length, + uint64_t timestamp) { + static jmethodID func = env->GetMethodID(canDataCls, "setData", "(IJ)[B"); + + jbyteArray retVal = static_cast( + env->CallObjectMethod(canData, func, (jint)length, (jlong)timestamp)); + return retVal; +} + JavaVM* GetJVM() { return jvm; } } // namespace frc @@ -314,6 +324,9 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) { accumulatorResultCls = JClass(env, "edu/wpi/first/wpilibj/AccumulatorResult"); if (!accumulatorResultCls) return JNI_ERR; + canDataCls = JClass(env, "edu/wpi/first/wpilibj/CANData"); + if (!canDataCls) return JNI_ERR; + return sim::SimOnLoad(vm, reserved); } @@ -338,6 +351,7 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) { canStatusCls.free(env); matchInfoDataCls.free(env); accumulatorResultCls.free(env); + canDataCls.free(env); jvm = nullptr; } diff --git a/hal/src/main/native/cpp/jni/HALUtil.h b/hal/src/main/native/cpp/jni/HALUtil.h index dd2cc3566b..7359d28539 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.h +++ b/hal/src/main/native/cpp/jni/HALUtil.h @@ -62,6 +62,9 @@ void SetMatchInfoObject(JNIEnv* env, jobject matchStatus, void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult, int64_t value, int64_t count); +jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length, + uint64_t timestamp); + JavaVM* GetJVM(); } // namespace frc diff --git a/hal/src/main/native/include/HAL/CANAPI.h b/hal/src/main/native/include/HAL/CANAPI.h new file mode 100644 index 0000000000..d996047709 --- /dev/null +++ b/hal/src/main/native/include/HAL/CANAPI.h @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "HAL/Types.h" + +enum HAL_CANDeviceType : int32_t { + HAL_CAN_Dev_kBroadcast = 0, + HAL_CAN_Dev_kRobotController = 1, + HAL_CAN_Dev_kMotorController = 2, + HAL_CAN_Dev_kRelayController = 3, + HAL_CAN_Dev_kGyroSensor = 4, + HAL_CAN_Dev_kAccelerometer = 5, + HAL_CAN_Dev_kUltrasonicSensor = 6, + HAL_CAN_Dev_kGearToothSensor = 7, + HAL_CAN_Dev_kPowerDistribution = 8, + HAL_CAN_Dev_kPneumatics = 9, + HAL_CAN_Dev_kMiscellaneous = 10, + HAL_CAN_Dev_kFirmwareUpdate = 31 +}; + +enum HAL_CANManufacturer : int32_t { + HAL_CAN_Man_kBroadcast = 0, + HAL_CAN_Man_kNI = 1, + HAL_CAN_Man_kLM = 2, + HAL_CAN_Man_kDEKA = 3, + HAL_CAN_Man_kCTRE = 4, + HAL_CAN_Man_kMS = 7, + HAL_CAN_Man_kTeamUse = 8, +}; + +#ifdef __cplusplus +extern "C" { +#endif + +HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, + int32_t deviceId, HAL_CANDeviceType deviceType, + int32_t* status); + +void HAL_CleanCAN(HAL_CANHandle handle); + +void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, int32_t* status); + +void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, + int32_t repeatMs, int32_t* status); + +void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId, + int32_t* status); + +void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status); + +void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status); + +void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t* status); +void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t periodMs, int32_t* status); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/hal/src/main/native/include/HAL/Errors.h b/hal/src/main/native/include/HAL/Errors.h index 3111a7ca2e..52f9374998 100644 --- a/hal/src/main/native/include/HAL/Errors.h +++ b/hal/src/main/native/include/HAL/Errors.h @@ -103,6 +103,9 @@ #define HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE \ "HAL: The priority requested to be set is invalid" +#define HAL_CAN_TIMEOUT -1154 +#define HAL_CAN_TIMEOUT_MESSAGE "HAL: CAN Receive has Timed Out" + #define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error"; #define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object" #define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked" diff --git a/hal/src/main/native/include/HAL/Types.h b/hal/src/main/native/include/HAL/Types.h index 702f5385c8..49d73b6aac 100644 --- a/hal/src/main/native/include/HAL/Types.h +++ b/hal/src/main/native/include/HAL/Types.h @@ -43,4 +43,6 @@ typedef HAL_Handle HAL_RelayHandle; typedef HAL_Handle HAL_SolenoidHandle; +typedef HAL_Handle HAL_CANHandle; + typedef int32_t HAL_Bool; diff --git a/hal/src/main/native/include/HAL/handles/HandlesInternal.h b/hal/src/main/native/include/HAL/handles/HandlesInternal.h index ddb962b945..c0f9ca2cf7 100644 --- a/hal/src/main/native/include/HAL/handles/HandlesInternal.h +++ b/hal/src/main/native/include/HAL/handles/HandlesInternal.h @@ -57,7 +57,8 @@ enum class HAL_HandleEnum { Solenoid = 15, AnalogGyro = 16, Vendor = 17, - SimulationJni = 18 + SimulationJni = 18, + CAN = 19, }; static inline int16_t getHandleIndex(HAL_Handle handle) { diff --git a/hal/src/main/native/sim/CANAPI.cpp b/hal/src/main/native/sim/CANAPI.cpp new file mode 100644 index 0000000000..07cfef16bd --- /dev/null +++ b/hal/src/main/native/sim/CANAPI.cpp @@ -0,0 +1,330 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/CANAPI.h" + +#include +#include + +#include + +#include "HAL/CAN.h" +#include "HAL/Errors.h" +#include "HAL/HAL.h" +#include "HAL/handles/UnlimitedHandleResource.h" + +using namespace hal; + +namespace { +struct Receives { + uint64_t lastTimeStamp; + uint8_t data[8]; + uint8_t length; +}; + +struct CANStorage { + HAL_CANManufacturer manufacturer; + HAL_CANDeviceType deviceType; + uint8_t deviceId; + wpi::mutex mapMutex; + wpi::SmallDenseMap periodicSends; + wpi::SmallDenseMap receives; +}; +} // namespace + +static UnlimitedHandleResource* + canHandles; + +static void CheckDeltaTime() { + // Noop on sim +} + +static inline uint64_t ConvertToFPGATime(uint32_t canMs) { + uint64_t canMsToUs = canMs * 1000; + return canMsToUs; +} + +namespace hal { +namespace init { +void InitializeCANAPI() { + static UnlimitedHandleResource + cH; + canHandles = &cH; +} +} // namespace init +} // namespace hal + +static int32_t CreateCANId(CANStorage* storage, int32_t apiId) { + int32_t createdId = 0; + createdId |= (static_cast(storage->deviceType) & 0x1F) << 24; + createdId |= (static_cast(storage->manufacturer) & 0xFF) << 16; + createdId |= (apiId & 0x3FF) << 6; + createdId |= (storage->deviceId & 0x3F); + return createdId; +} + +HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer, + int32_t deviceId, HAL_CANDeviceType deviceType, + int32_t* status) { + CheckDeltaTime(); + auto can = std::make_shared(); + + auto handle = canHandles->Allocate(can); + + if (handle == HAL_kInvalidHandle) { + *status = NO_AVAILABLE_RESOURCES; + return HAL_kInvalidHandle; + } + + can->deviceId = deviceId; + can->deviceType = deviceType; + can->manufacturer = manufacturer; + + return handle; +} + +void HAL_CleanCAN(HAL_CANHandle handle) { + auto data = canHandles->Free(handle); + + std::lock_guard lock(data->mapMutex); + + for (auto&& i : data->periodicSends) { + int32_t s = 0; + HAL_CAN_SendMessage(i.first, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, + &s); + i.second = -1; + } +} + +void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = -1; +} + +void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data, + int32_t length, int32_t apiId, + int32_t repeatMs, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, data, length, repeatMs, status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = repeatMs; +} + +void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING, + status); + + if (*status != 0) { + return; + } + std::lock_guard lock(can->mapMutex); + can->periodicSends[apiId] = -1; +} + +void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + if (*status == 0) { + std::lock_guard lock(can->mapMutex); + auto& msg = can->receives[id]; + msg.length = dataSize; + msg.lastTimeStamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } + *length = dataSize; + *receivedTimestamp = timestamp; +} + +void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data, + int32_t* length, uint64_t* receivedTimestamp, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + // Found, check if new enough + uint64_t now = HAL_GetFPGATime(status); + if (now - i->second.lastTimeStamp > + static_cast(timeoutMs) * 1000) { + // Timeout, return bad status + *status = HAL_CAN_TIMEOUT; + return; + } + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} + +void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId, + uint8_t* data, int32_t* length, + uint64_t* receivedTimestamp, int32_t timeoutMs, + int32_t periodMs, int32_t* status) { + auto can = canHandles->Get(handle); + if (!can) { + *status = HAL_HANDLE_ERROR; + return; + } + auto id = CreateCANId(can.get(), apiId); + + { + std::lock_guard lock(can->mapMutex); + auto i = can->receives.find(id); + if (i != can->receives.end()) { + uint64_t now = HAL_GetFPGATime(status); + // Found, check if new enough + if (now - i->second.lastTimeStamp < + static_cast(periodMs) * 1000) { + *status = 0; + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + } + } + } + + uint32_t messageId = 0; + uint8_t dataSize = 0; + uint32_t ts = 0; + HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status); + + uint64_t timestamp = ConvertToFPGATime(ts); + + std::lock_guard lock(can->mapMutex); + if (*status == 0) { + // fresh update + auto& msg = can->receives[id]; + msg.length = dataSize; + *length = dataSize; + msg.lastTimeStamp = timestamp; + *receivedTimestamp = timestamp; + std::memcpy(msg.data, data, dataSize); + } else { + auto i = can->receives.find(id); + if (i != can->receives.end()) { + // Found, check if new enough + uint64_t now = HAL_GetFPGATime(status); + if (now - i->second.lastTimeStamp > + static_cast(timeoutMs) * 1000) { + // Timeout, return bad status + *status = HAL_CAN_TIMEOUT; + return; + } + std::memcpy(i->second.data, data, i->second.length); + *length = i->second.length; + *receivedTimestamp = i->second.lastTimeStamp; + *status = 0; + } + } +} diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 9799cff16b..66c24b1839 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -29,6 +29,7 @@ void InitializeHAL() { InitializeAnalogOutData(); InitializeAnalogTriggerData(); InitializeCanData(); + InitializeCANAPI(); InitializeDigitalPWMData(); InitializeDIOData(); InitializeDriverStationData(); @@ -196,6 +197,8 @@ const char* HAL_GetErrorMessage(int32_t code) { return VI_ERROR_INV_PARAMETER_MESSAGE; case HAL_PWM_SCALE_ERROR: return HAL_PWM_SCALE_ERROR_MESSAGE; + case HAL_CAN_TIMEOUT: + return HAL_CAN_TIMEOUT_MESSAGE; default: return "Unknown error status"; } diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h index 7bcddb6ea0..886addd24b 100644 --- a/hal/src/main/native/sim/HALInitializer.h +++ b/hal/src/main/native/sim/HALInitializer.h @@ -24,6 +24,7 @@ extern void InitializeAnalogInData(); extern void InitializeAnalogOutData(); extern void InitializeAnalogTriggerData(); extern void InitializeCanData(); +extern void InitializeCANAPI(); extern void InitializeDigitalPWMData(); extern void InitializeDIOData(); extern void InitializeDriverStationData(); diff --git a/hal/src/test/native/cpp/can/CANTest.cpp b/hal/src/test/native/cpp/can/CANTest.cpp new file mode 100644 index 0000000000..410a32cb38 --- /dev/null +++ b/hal/src/test/native/cpp/can/CANTest.cpp @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2015-2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "HAL/CANAPI.h" +#include "HAL/HAL.h" +#include "MockData/CanData.h" +#include "gtest/gtest.h" + +namespace hal { +struct CANTestStore { + CANTestStore(int32_t deviceId, int32_t* status) { + this->deviceId = deviceId; + handle = HAL_InitializeCAN( + HAL_CANManufacturer::HAL_CAN_Man_kTeamUse, deviceId, + HAL_CANDeviceType::HAL_CAN_Dev_kMiscellaneous, status); + } + + ~CANTestStore() { + if (handle != HAL_kInvalidHandle) { + HAL_CleanCAN(handle); + } + } + + int32_t deviceId; + HAL_CANHandle handle; +}; + +struct CANReceiveCallbackStore { + explicit CANReceiveCallbackStore(int32_t handle) { this->handle = handle; } + ~CANReceiveCallbackStore() { HALSIM_CancelCanReceiveMessageCallback(handle); } + int32_t handle; +}; + +struct CANSendCallbackStore { + explicit CANSendCallbackStore(int32_t handle) { this->handle = handle; } + ~CANSendCallbackStore() { HALSIM_CancelCanSendMessageCallback(handle); } + int32_t handle; +}; + +TEST(HALCanTests, CanIdPackingTest) { + int32_t status = 0; + int32_t deviceId = 12; + CANTestStore testStore(deviceId, &status); + ASSERT_EQ(0, status); + + std::pair storePair; + storePair.second = false; + + auto cbHandle = HALSIM_RegisterCanSendMessageCallback( + [](const char* name, void* param, uint32_t messageID, const uint8_t* data, + uint8_t dataSize, int32_t periodMs, int32_t* status) { + std::pair* paramI = + reinterpret_cast*>(param); + paramI->first = messageID; + paramI->second = true; + }, + &storePair); + + CANSendCallbackStore cbStore(cbHandle); + uint8_t data[8]; + + int32_t apiId = 42; + + HAL_WriteCANPacket(testStore.handle, data, 8, 42, &status); + + ASSERT_EQ(0, status); + + ASSERT_TRUE(storePair.second); + + ASSERT_NE(0, storePair.first); + + ASSERT_EQ(deviceId, storePair.first & 0x3F); + ASSERT_EQ(apiId, (storePair.first & 0x0000FFC0) >> 6); + ASSERT_EQ(static_cast(HAL_CANManufacturer::HAL_CAN_Man_kTeamUse), + (storePair.first & 0x00FF0000) >> 16); + ASSERT_EQ(static_cast(HAL_CANDeviceType::HAL_CAN_Dev_kMiscellaneous), + (storePair.first & 0x1F000000) >> 24); +} +} // namespace hal diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp new file mode 100644 index 0000000000..ea5d00dcc6 --- /dev/null +++ b/wpilibc/src/main/native/cpp/CAN.cpp @@ -0,0 +1,183 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "CAN.h" + +#include + +using namespace frc; + +/** + * Create a new CAN communication interface with the specific device ID. + * The device ID is 6 bits (0-63) + */ +CAN::CAN(int deviceId) { + int32_t status = 0; + m_handle = + HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status); + if (status != 0) { + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + m_handle = HAL_kInvalidHandle; + return; + } + + // HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId); +} + +/** + * Closes the CAN communication. + */ +CAN::~CAN() { + if (StatusIsFatal()) return; + if (m_handle != HAL_kInvalidHandle) { + HAL_CleanCAN(m_handle); + m_handle = HAL_kInvalidHandle; + } +} + +/** + * Write a packet to the CAN device with a specific ID. This ID is 10 bits. + * + * @param data The data to write (8 bytes max) + * @param length The data length to write + * @param apiId The API ID to write. + */ +void CAN::WritePacket(const uint8_t* data, int length, int apiId) { + int32_t status = 0; + HAL_WriteCANPacket(m_handle, data, length, apiId, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Write a repeating packet to the CAN device with a specific ID. This ID is 10 + * bits. The RoboRIO will automatically repeat the packet at the specified + * interval + * + * @param data The data to write (8 bytes max) + * @param length The data length to write + * @param apiId The API ID to write. + * @param repeatMs The period to repeat the packet at. + */ +void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId, + int repeatMs) { + int32_t status = 0; + HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Stop a repeating packet with a specific ID. This ID is 10 bits. + * + * @param apiId The API ID to stop repeating + */ +void CAN::StopPacketRepeating(int apiId) { + int32_t status = 0; + HAL_StopCANPacketRepeating(m_handle, apiId, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} + +/** + * Read a new CAN packet. This will only return properly once per packet + * received. Multiple calls without receiving another packet will return false. + * + * @param apiId The API ID to read. + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ +bool CAN::ReadPacketNew(int apiId, CANData* data) { + int32_t status = 0; + HAL_ReadCANPacketNew(m_handle, apiId, data->data, &data->length, + &data->timestamp, &status); + if (status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (status != 0) { + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return false; + } else { + return true; + } +} + +/** + * Read a CAN packet. The will continuously return the last packet received, + * without accounting for packet age. + * + * @param apiId The API ID to read. + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ +bool CAN::ReadPacketLatest(int apiId, CANData* data) { + int32_t status = 0; + HAL_ReadCANPacketLatest(m_handle, apiId, data->data, &data->length, + &data->timestamp, &status); + if (status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (status != 0) { + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return false; + } else { + return true; + } +} + +/** + * Read a CAN packet. The will return the last packet received until the + * packet is older then the requested timeout. Then it will return false. + * + * @param apiId The API ID to read. + * @param timeoutMs The timeout time for the packet + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ +bool CAN::ReadPacketTimeout(int apiId, int timeoutMs, CANData* data) { + int32_t status = 0; + HAL_ReadCANPacketTimeout(m_handle, apiId, data->data, &data->length, + &data->timestamp, timeoutMs, &status); + if (status == HAL_CAN_TIMEOUT || + status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (status != 0) { + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return false; + } else { + return true; + } +} + +/** + * Read a CAN packet. The will return the last packet received until the + * packet is older then the requested timeout. Then it will return false. + * The period parameter is used when you know the packet is sent at specific + * intervals, so calls will not attempt to read a new packet from the + * network until that period has passed. We do not recommend users use this + * API unless they know the implications. + * + * @param apiId The API ID to read. + * @param timeoutMs The timeout time for the packet + * @param periodMs The usual period for the packet + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ +bool CAN::ReadPeriodicPacket(int apiId, int timeoutMs, int periodMs, + CANData* data) { + int32_t status = 0; + HAL_ReadCANPeriodicPacket(m_handle, apiId, data->data, &data->length, + &data->timestamp, timeoutMs, periodMs, &status); + if (status == HAL_CAN_TIMEOUT || + status == HAL_ERR_CANSessionMux_MessageNotFound) { + return false; + } + if (status != 0) { + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + return false; + } else { + return true; + } +} diff --git a/wpilibc/src/main/native/include/CAN.h b/wpilibc/src/main/native/include/CAN.h new file mode 100644 index 0000000000..600d6eb6a0 --- /dev/null +++ b/wpilibc/src/main/native/include/CAN.h @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include + +#include "ErrorBase.h" + +namespace frc { +struct CANData { + uint8_t data[8]; + int32_t length; + uint64_t timestamp; +}; + +/** + * High level class for interfacing with CAN devices conforming to + * the standard CAN spec. + * + * No packets that can be sent gets blocked by the RoboRIO, so all methods + * work identically in all robot modes. + * + * All methods are thread save, however the buffer objects passed in + * by the user need to not be modified for the duration of their calls. + */ +class CAN : public ErrorBase { + public: + explicit CAN(int deviceId); + ~CAN() override; + + void WritePacket(const uint8_t* data, int length, int apiId); + void WritePacketRepeating(const uint8_t* data, int length, int apiId, + int repeatMs); + void StopPacketRepeating(int apiId); + + bool ReadPacketNew(int apiId, CANData* data); + bool ReadPacketLatest(int apiId, CANData* data); + bool ReadPacketTimeout(int apiId, int timeoutMs, CANData* data); + bool ReadPeriodicPacket(int apiId, int timeoutMs, int periodMs, + CANData* data); + + static constexpr HAL_CANManufacturer kTeamManufacturer = HAL_CAN_Man_kTeamUse; + static constexpr HAL_CANDeviceType kTeamDeviceType = + HAL_CAN_Dev_kMiscellaneous; + + private: + HAL_CANHandle m_handle{HAL_kInvalidHandle}; +}; +} // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java new file mode 100644 index 0000000000..d9be432218 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java @@ -0,0 +1,134 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import java.io.Closeable; + +import edu.wpi.first.wpilibj.hal.CANAPIJNI; + +/** + * High level class for interfacing with CAN devices conforming to + * the standard CAN spec. + * + *

No packets that can be sent gets blocked by the RoboRIO, so all methods + * work identically in all robot modes. + * + *

All methods are thread safe, however the CANData object passed into the + * read methods and the byte[] passed into the write methods need to not + * be modified for the duration of their respective calls. + */ +public class CAN implements Closeable { + public static final int kTeamManufacturer = 8; + public static final int kTeamDeviceType = 10; + + private int m_handle; + + /** + * Create a new CAN communication interface with the specific device ID. + * The device ID is 6 bits (0-63) + */ + public CAN(int deviceId) { + m_handle = CANAPIJNI.initializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType); + } + + /** + * Closes the CAN communication. + */ + @Override + public void close() { + if (m_handle != 0) { + CANAPIJNI.cleanCAN(m_handle); + } + } + + /** + * Write a packet to the CAN device with a specific ID. This ID is 10 bits. + * + * @param data The data to write (8 bytes max) + * @param apiId The API ID to write. + */ + public void writePacket(byte[] data, int apiId) { + CANAPIJNI.writeCANPacket(m_handle, data, apiId); + } + + /** + * Write a repeating packet to the CAN device with a specific ID. This ID is 10 bits. + * The RoboRIO will automatically repeat the packet at the specified interval + * + * @param data The data to write (8 bytes max) + * @param apiId The API ID to write. + * @param repeatMs The period to repeat the packet at. + */ + public void writePacketRepeating(byte[] data, int apiId, int repeatMs) { + CANAPIJNI.writeCANPacketRepeating(m_handle, data, apiId, repeatMs); + } + + /** + * Stop a repeating packet with a specific ID. This ID is 10 bits. + * + * @param apiId The API ID to stop repeating + */ + public void stopPacketRepeating(int apiId) { + CANAPIJNI.stopCANPacketRepeating(m_handle, apiId); + } + + /** + * Read a new CAN packet. This will only return properly once per packet + * received. Multiple calls without receiving another packet will return false. + * + * @param apiId The API ID to read. + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ + public boolean readPacketNew(int apiId, CANData data) { + return CANAPIJNI.readCANPacketNew(m_handle, apiId, data); + } + + /** + * Read a CAN packet. The will continuously return the last packet received, + * without accounting for packet age. + * + * @param apiId The API ID to read. + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ + public boolean readPacketLatest(int apiId, CANData data) { + return CANAPIJNI.readCANPacketLatest(m_handle, apiId, data); + } + + /** + * Read a CAN packet. The will return the last packet received until the + * packet is older then the requested timeout. Then it will return false. + * + * @param apiId The API ID to read. + * @param timeoutMs The timeout time for the packet + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ + public boolean readPacketTimeout(int apiId, int timeoutMs, CANData data) { + return CANAPIJNI.readCANPacketTimeout(m_handle, apiId, timeoutMs, data); + } + + /** + * Read a CAN packet. The will return the last packet received until the + * packet is older then the requested timeout. Then it will return false. + * The period parameter is used when you know the packet is sent at specific + * intervals, so calls will not attempt to read a new packet from the + * network until that period has passed. We do not recommend users use this + * API unless they know the implications. + * + * @param apiId The API ID to read. + * @param timeoutMs The timeout time for the packet + * @param periodMs The usual period for the packet + * @param data Storage for the received data. + * @return True if the data is valid, otherwise false. + */ + public boolean readPeriodicPacket(int apiId, int timeoutMs, int periodMs, CANData data) { + return CANAPIJNI.readCANPeriodicPacket(m_handle, apiId, timeoutMs, periodMs, data); + } +}