From b3c4ad8a5685d9724cb7b79c5522bf24ca12af8c Mon Sep 17 00:00:00 2001 From: Will Toth Date: Mon, 23 Mar 2020 14:12:33 -0500 Subject: [PATCH 01/14] Start of SocketCAN implementation --- .vscode/settings.json | 17 +- .wpilib/wpilib_preferences.json | 4 +- gradlew | 0 .../com/revrobotics/jni/RevJNIWrapper.java | 2 +- src/main/native/cpp/CANBridge.cpp | 2 +- src/main/native/cpp/CANBridgeUtils.cpp | 2 +- .../CandleWinUSB/CandleWinUSBDevice.cpp | 2 +- .../CandleWinUSB/CandleWinUSBDriver.cpp | 2 +- .../cpp/Drivers/Serial/SerialDevice.cpp | 2 +- .../cpp/Drivers/Serial/SerialDriver.cpp | 2 +- .../cpp/Drivers/Serial/SerialMessage.cpp | 2 +- .../cpp/Drivers/SocketCAN/SocketCANDriver.cpp | 65 +++++ src/main/native/cpp/ThreadUtils.cpp | 2 +- src/main/native/cpp/can-utils/canframelen.c | 263 ++++++++++++++++++ .../native/include/can-utils/canframelen.h | 93 +++++++ src/main/native/include/rev/CANBridge.h | 2 +- src/main/native/include/rev/CANBridgeUtils.h | 2 +- src/main/native/include/rev/CANDevice.h | 2 +- src/main/native/include/rev/CANDriver.h | 2 +- src/main/native/include/rev/CANMessage.h | 2 +- src/main/native/include/rev/CANStatus.h | 2 +- .../Drivers/CandleWinUSB/CandleWinUSBDevice.h | 2 +- .../CandleWinUSB/CandleWinUSBDeviceThread.h | 2 +- .../Drivers/CandleWinUSB/CandleWinUSBDriver.h | 2 +- .../include/rev/Drivers/DriverDeviceThread.h | 2 +- .../rev/Drivers/SerialPort/SerialDevice.h | 2 +- .../Drivers/SerialPort/SerialDeviceThread.h | 2 +- .../rev/Drivers/SerialPort/SerialDriver.h | 2 +- .../rev/Drivers/SerialPort/SerialMessage.h | 2 +- .../rev/Drivers/SocketCAN/SocketCANDevice.h | 71 +++++ .../rev/Drivers/SocketCAN/SocketCANDriver.h | 51 ++++ .../rev/Drivers/SocketCAN/SocketCANThread.h | 217 +++++++++++++++ .../native/include/utils/CircularBuffer.h | 2 +- src/main/native/include/utils/ThreadUtils.h | 2 +- src/test/gtest/cpp/main.cpp | 2 +- 35 files changed, 804 insertions(+), 29 deletions(-) mode change 100644 => 100755 gradlew create mode 100644 src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp create mode 100644 src/main/native/cpp/can-utils/canframelen.c create mode 100644 src/main/native/include/can-utils/canframelen.h create mode 100644 src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h create mode 100644 src/main/native/include/rev/Drivers/SocketCAN/SocketCANDriver.h create mode 100644 src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h diff --git a/.vscode/settings.json b/.vscode/settings.json index 47280d7..29cd40d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,7 +68,22 @@ "iterator": "cpp", "sstream": "cpp", "cctype": "cpp", - "concepts": "cpp" + "concepts": "cpp", + "inet.h": "c", + "cwctype": "cpp", + "array": "cpp", + "hash_map": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "set": "cpp", + "string_view": "cpp", + "cinttypes": "cpp" }, "C_Cpp.default.configurationProvider": "vscode-wpilib" } \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index 8a51c0a..fc2a570 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "enableCppIntellisense": true, "currentLanguage": "cpp", - "projectYear": "Beta2020-2", + "projectYear": "2020", "teamNumber": 9999 -} +} \ No newline at end of file diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/com/revrobotics/jni/RevJNIWrapper.java b/src/main/java/com/revrobotics/jni/RevJNIWrapper.java index db3fdab..f6dfe4b 100644 --- a/src/main/java/com/revrobotics/jni/RevJNIWrapper.java +++ b/src/main/java/com/revrobotics/jni/RevJNIWrapper.java @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/CANBridge.cpp b/src/main/native/cpp/CANBridge.cpp index 7a1e566..33fe438 100644 --- a/src/main/native/cpp/CANBridge.cpp +++ b/src/main/native/cpp/CANBridge.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/CANBridgeUtils.cpp b/src/main/native/cpp/CANBridgeUtils.cpp index a820183..881591e 100644 --- a/src/main/native/cpp/CANBridgeUtils.cpp +++ b/src/main/native/cpp/CANBridgeUtils.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp b/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp index 64764a2..f047e99 100644 --- a/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp +++ b/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDevice.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDriver.cpp b/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDriver.cpp index f4594a7..bb3452b 100644 --- a/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDriver.cpp +++ b/src/main/native/cpp/Drivers/CandleWinUSB/CandleWinUSBDriver.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp b/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp index 41d86f7..a56f0f5 100644 --- a/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp +++ b/src/main/native/cpp/Drivers/Serial/SerialDevice.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/Serial/SerialDriver.cpp b/src/main/native/cpp/Drivers/Serial/SerialDriver.cpp index 1be761f..13de544 100644 --- a/src/main/native/cpp/Drivers/Serial/SerialDriver.cpp +++ b/src/main/native/cpp/Drivers/Serial/SerialDriver.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/Serial/SerialMessage.cpp b/src/main/native/cpp/Drivers/Serial/SerialMessage.cpp index 983aec7..d802ecc 100644 --- a/src/main/native/cpp/Drivers/Serial/SerialMessage.cpp +++ b/src/main/native/cpp/Drivers/Serial/SerialMessage.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp new file mode 100644 index 0000000..d1ce4f6 --- /dev/null +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2019 - 2020 REV Robotics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of REV Robotics nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __linux__ + +#include "rev/Drivers/SocketCAN/SocketCANDriver.h" +#include "rev/Drivers/SocketCAN/SocketCANDevice.h" + +#include +#include +#include + +namespace rev { +namespace usb { + +std::vector SocketCANDriver::GetDevices() +{ + std::vector retval; + + return retval; +} + +std::unique_ptr SocketCANDriver::CreateDeviceFromDescriptor(const char* descriptor) +{ + uint8_t num_interfaces; + + try { + return std::make_unique(descriptor); + } catch(...) { + // do nothing if it failed + } + return std::unique_ptr(); +} + +} // namespace usb +} // namespace rev + +#else +typedef int __ISOWarning__CLEAR_; +#endif // _WIN32 diff --git a/src/main/native/cpp/ThreadUtils.cpp b/src/main/native/cpp/ThreadUtils.cpp index 2d9e9b3..1bce0f7 100644 --- a/src/main/native/cpp/ThreadUtils.cpp +++ b/src/main/native/cpp/ThreadUtils.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/cpp/can-utils/canframelen.c b/src/main/native/cpp/can-utils/canframelen.c new file mode 100644 index 0000000..da509eb --- /dev/null +++ b/src/main/native/cpp/can-utils/canframelen.c @@ -0,0 +1,263 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * canframelen.c + * + * Copyright (c) 2013, 2014 Czech Technical University in Prague + * + * Author: Michal Sojka + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Czech Technical University in Prague nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#include "canframelen.h" +#include +#include +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Functions and types for CRC checks. + * + * Generated on Wed Jan 8 15:14:20 2014, + * by pycrc v0.8.1, http://www.tty1.net/pycrc/ + * using the configuration: + * Width = 15 + * Poly = 0x4599 + * XorIn = 0x0000 + * ReflectIn = False + * XorOut = 0x0000 + * ReflectOut = False + * Algorithm = table-driven + *****************************************************************************/ + +typedef uint16_t crc_t; + +/** + * Static table used for the table_driven implementation. + *****************************************************************************/ +static const crc_t crc_table[256] = { + 0x0000, 0x4599, 0x4eab, 0x0b32, 0x58cf, 0x1d56, 0x1664, 0x53fd, 0x7407, 0x319e, 0x3aac, 0x7f35, 0x2cc8, 0x6951, 0x6263, 0x27fa, + 0x2d97, 0x680e, 0x633c, 0x26a5, 0x7558, 0x30c1, 0x3bf3, 0x7e6a, 0x5990, 0x1c09, 0x173b, 0x52a2, 0x015f, 0x44c6, 0x4ff4, 0x0a6d, + 0x5b2e, 0x1eb7, 0x1585, 0x501c, 0x03e1, 0x4678, 0x4d4a, 0x08d3, 0x2f29, 0x6ab0, 0x6182, 0x241b, 0x77e6, 0x327f, 0x394d, 0x7cd4, + 0x76b9, 0x3320, 0x3812, 0x7d8b, 0x2e76, 0x6bef, 0x60dd, 0x2544, 0x02be, 0x4727, 0x4c15, 0x098c, 0x5a71, 0x1fe8, 0x14da, 0x5143, + 0x73c5, 0x365c, 0x3d6e, 0x78f7, 0x2b0a, 0x6e93, 0x65a1, 0x2038, 0x07c2, 0x425b, 0x4969, 0x0cf0, 0x5f0d, 0x1a94, 0x11a6, 0x543f, + 0x5e52, 0x1bcb, 0x10f9, 0x5560, 0x069d, 0x4304, 0x4836, 0x0daf, 0x2a55, 0x6fcc, 0x64fe, 0x2167, 0x729a, 0x3703, 0x3c31, 0x79a8, + 0x28eb, 0x6d72, 0x6640, 0x23d9, 0x7024, 0x35bd, 0x3e8f, 0x7b16, 0x5cec, 0x1975, 0x1247, 0x57de, 0x0423, 0x41ba, 0x4a88, 0x0f11, + 0x057c, 0x40e5, 0x4bd7, 0x0e4e, 0x5db3, 0x182a, 0x1318, 0x5681, 0x717b, 0x34e2, 0x3fd0, 0x7a49, 0x29b4, 0x6c2d, 0x671f, 0x2286, + 0x2213, 0x678a, 0x6cb8, 0x2921, 0x7adc, 0x3f45, 0x3477, 0x71ee, 0x5614, 0x138d, 0x18bf, 0x5d26, 0x0edb, 0x4b42, 0x4070, 0x05e9, + 0x0f84, 0x4a1d, 0x412f, 0x04b6, 0x574b, 0x12d2, 0x19e0, 0x5c79, 0x7b83, 0x3e1a, 0x3528, 0x70b1, 0x234c, 0x66d5, 0x6de7, 0x287e, + 0x793d, 0x3ca4, 0x3796, 0x720f, 0x21f2, 0x646b, 0x6f59, 0x2ac0, 0x0d3a, 0x48a3, 0x4391, 0x0608, 0x55f5, 0x106c, 0x1b5e, 0x5ec7, + 0x54aa, 0x1133, 0x1a01, 0x5f98, 0x0c65, 0x49fc, 0x42ce, 0x0757, 0x20ad, 0x6534, 0x6e06, 0x2b9f, 0x7862, 0x3dfb, 0x36c9, 0x7350, + 0x51d6, 0x144f, 0x1f7d, 0x5ae4, 0x0919, 0x4c80, 0x47b2, 0x022b, 0x25d1, 0x6048, 0x6b7a, 0x2ee3, 0x7d1e, 0x3887, 0x33b5, 0x762c, + 0x7c41, 0x39d8, 0x32ea, 0x7773, 0x248e, 0x6117, 0x6a25, 0x2fbc, 0x0846, 0x4ddf, 0x46ed, 0x0374, 0x5089, 0x1510, 0x1e22, 0x5bbb, + 0x0af8, 0x4f61, 0x4453, 0x01ca, 0x5237, 0x17ae, 0x1c9c, 0x5905, 0x7eff, 0x3b66, 0x3054, 0x75cd, 0x2630, 0x63a9, 0x689b, 0x2d02, + 0x276f, 0x62f6, 0x69c4, 0x2c5d, 0x7fa0, 0x3a39, 0x310b, 0x7492, 0x5368, 0x16f1, 0x1dc3, 0x585a, 0x0ba7, 0x4e3e, 0x450c, 0x0095 +}; + +/** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data Pointer to a buffer of \a data_len bytes. + * \param data_len Number of bytes in the \a data buffer. + * \return The updated crc value. + *****************************************************************************/ +static crc_t crc_update_bytewise(crc_t crc, const unsigned char *data, size_t data_len) +{ + unsigned int tbl_idx; + + while (data_len--) { + tbl_idx = ((crc >> 7) ^ *data) & 0xff; + crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0x7fff; + + data++; + } + return crc & 0x7fff; +} + +/** + * Update the crc value with new data. + * + * \param crc The current crc value. + * \param data Data value + * \param bits The number of most significant bits in data used for CRC calculation + * \return The updated crc value. + *****************************************************************************/ +static crc_t crc_update_bitwise(crc_t crc, uint8_t data, size_t bits) +{ + uint8_t i; + bool bit; + + for (i = 0x80; bits--; i >>= 1) { + bit = crc & 0x4000; + if (data & i) { + bit = !bit; + } + crc <<= 1; + if (bit) { + crc ^= 0x4599; + } + } + return crc & 0x7fff; +} + +static crc_t calc_bitmap_crc(uint8_t *bitmap, unsigned start, unsigned end) +{ + crc_t crc = 0; + + if (start % 8) { + crc = crc_update_bitwise(crc, bitmap[start / 8] << (start % 8), 8 - start % 8); + start += 8 - start % 8; + } + crc = crc_update_bytewise(crc, &bitmap[start / 8], (end - start) / 8); + crc = crc_update_bitwise(crc, bitmap[end / 8], end % 8); + return crc; +} + +static unsigned cfl_exact(struct can_frame *frame) +{ + uint8_t bitmap[16]; + unsigned start = 0, end; + crc_t crc; + uint16_t crc_be; + uint8_t mask, lookfor; + unsigned i, stuffed; + const int8_t clz[32] = /* count of leading zeros in 5 bit numbers */ + { 5, 4, 3, 3, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + /* Prepare bitmap */ + memset(bitmap, 0, sizeof(bitmap)); + if (frame->can_id & CAN_EFF_FLAG) { + /* bit 7 0 7 0 7 0 7 0 + * bitmap[0-3] |.sBBBBBB BBBBBSIE EEEEEEEE EEEEEEEE| s = SOF, B = Base ID (11 bits), S = SRR, I = IDE, E = Extended ID (18 bits) + * bitmap[4-7] |ER10DLC4 00000000 11111111 22222222| R = RTR, 0 = r0, 1 = r1, DLC4 = DLC, Data bytes + * bitmap[8-11] |33333333 44444444 55555555 66666666| Data bytes + * bitmap[12-15] |77777777 ........ ........ ........| Data bytes + */ + bitmap[0] = (frame->can_id & CAN_EFF_MASK) >> 23; + bitmap[1] = ((frame->can_id >> 18) & 0x3f) << 3 | + 3 << 1 | /* SRR, IDE */ + ((frame->can_id >> 17) & 0x01); + bitmap[2] = (frame->can_id >> 9) & 0xff; + bitmap[3] = (frame->can_id >> 1) & 0xff; + bitmap[4] = (frame->can_id & 0x1) << 7 | + (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | + 0 << 4 | /* r1, r0 */ + (frame->can_dlc & 0xf); + memcpy(&bitmap[5], &frame->data, frame->can_dlc); + start = 1; + end = 40 + 8*frame->can_dlc; + } else { + /* bit 7 0 7 0 7 0 7 0 + * bitmap[0-3] |.....sII IIIIIIII IRE0DLC4 00000000| s = SOF, I = ID (11 bits), R = RTR, E = IDE, DLC4 = DLC + * bitmap[4-7] |11111111 22222222 33333333 44444444| Data bytes + * bitmap[8-11] |55555555 66666666 77777777 ........| Data bytes + */ + bitmap[0] = (frame->can_id & CAN_SFF_MASK) >> 9; + bitmap[1] = (frame->can_id >> 1) & 0xff; + bitmap[2] = ((frame->can_id << 7) & 0xff) | + (!!(frame->can_id & CAN_RTR_FLAG)) << 6 | + 0 << 4 | /* IDE, r0 */ + (frame->can_dlc & 0xf); + memcpy(&bitmap[3], &frame->data, frame->can_dlc); + start = 5; + end = 24 + 8 * frame->can_dlc; + } + + /* Calc and append CRC */ + crc = calc_bitmap_crc(bitmap, start, end); + crc_be = htons(crc << 1); + assert(end % 8 == 0); + memcpy(bitmap + end / 8, &crc_be, 2); + end += 15; + + /* Count stuffed bits */ + mask = 0x1f; + lookfor = 0; + i = start; + stuffed = 0; + while (i < end) { + unsigned change; + unsigned bits = (bitmap[i / 8] << 8 | bitmap[i / 8 + 1]) >> (16 - 5 - i % 8); + lookfor = lookfor ? 0 : mask; /* We alternate between looking for a series of zeros or ones */ + change = (bits & mask) ^ lookfor; /* 1 indicates a change */ + if (change) { /* No bit was stuffed here */ + i += clz[change]; + mask = 0x1f; /* Next look for 5 same bits */ + } else { + i += (mask == 0x1f) ? 5 : 4; + if (i <= end) { + stuffed++; + mask = 0x1e; /* Next look for 4 bits (5th bit is the stuffed one) */ + } + } + } + return end - start + stuffed + + 3 + /* CRC del, ACK, ACK del */ + 7 + /* EOF */ + 3; /* IFS */ +} + + +unsigned can_frame_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu) +{ + int eff = (frame->can_id & CAN_EFF_FLAG); + + if (mtu != CAN_MTU) + return 0; /* CANFD is not supported yet */ + + switch (mode) { + case CFL_NO_BITSTUFFING: + return (eff ? 67 : 47) + frame->len * 8; + case CFL_WORSTCASE: + return (eff ? 80 : 55) + frame->len * 10; + case CFL_EXACT: + return cfl_exact((struct can_frame*)frame); + } + return 0; /* Unknown mode */ +} + +#ifdef __cplusplus +} +#endif diff --git a/src/main/native/include/can-utils/canframelen.h b/src/main/native/include/can-utils/canframelen.h new file mode 100644 index 0000000..0384b28 --- /dev/null +++ b/src/main/native/include/can-utils/canframelen.h @@ -0,0 +1,93 @@ +/* SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause) */ +/* + * canframelen.h + * + * Copyright (c) 2013, 2014 Czech Technical University in Prague + * + * Author: Michal Sojka + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of Czech Technical University in Prague nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * Alternatively, provided that this notice is retained in full, this + * software may be distributed under the terms of the GNU General + * Public License ("GPL") version 2, in which case the provisions of the + * GPL apply INSTEAD OF those given above. + * + * The provided data structures and external interfaces from this code + * are not restricted to be used by modules with a GPL compatible license. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * Send feedback to + * + */ + +#ifndef CANFRAMELEN_H +#define CANFRAMELEN_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Frame length calculation modes. + * + * CFL_WORSTCASE corresponds to *worst* case calculation for + * stuff-bits - see (1)-(3) in [1]. The worst case number of bits on + * the wire can be calculated as: + * + * (34 + 8n - 1)/4 + 34 + 8n + 13 for SFF frames (11 bit CAN-ID) => 55 + 10n + * (54 + 8n - 1)/4 + 54 + 8n + 13 for EFF frames (29 bit CAN-ID) => 80 + 10n + * + * while 'n' is the data length code (number of payload bytes) + * + * [1] "Controller Area Network (CAN) schedulability analysis: + * Refuted, revisited and revised", Real-Time Syst (2007) + * 35:239-272. + * + */ +enum cfl_mode { + CFL_NO_BITSTUFFING, /* plain bit calculation without bitstuffing */ + CFL_WORSTCASE, /* worst case estimation - see above */ + CFL_EXACT, /* exact calculation of stuffed bits based on frame + * content and CRC */ +}; + +/** + * Calculates the number of bits a frame needs on the wire (including + * inter frame space). + * + * Mode determines how to deal with stuffed bits. + */ +unsigned can_frame_length(struct canfd_frame *frame, enum cfl_mode mode, int mtu); + +#ifdef __cplusplus +} +#endif + +#endif + diff --git a/src/main/native/include/rev/CANBridge.h b/src/main/native/include/rev/CANBridge.h index 3047b50..6e6af04 100644 --- a/src/main/native/include/rev/CANBridge.h +++ b/src/main/native/include/rev/CANBridge.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/CANBridgeUtils.h b/src/main/native/include/rev/CANBridgeUtils.h index 6431ff0..ec19dae 100644 --- a/src/main/native/include/rev/CANBridgeUtils.h +++ b/src/main/native/include/rev/CANBridgeUtils.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/CANDevice.h b/src/main/native/include/rev/CANDevice.h index 7d79bb9..65901eb 100644 --- a/src/main/native/include/rev/CANDevice.h +++ b/src/main/native/include/rev/CANDevice.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/CANDriver.h b/src/main/native/include/rev/CANDriver.h index 937a85d..fc58bbe 100644 --- a/src/main/native/include/rev/CANDriver.h +++ b/src/main/native/include/rev/CANDriver.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/CANMessage.h b/src/main/native/include/rev/CANMessage.h index 2d75565..98e3f03 100644 --- a/src/main/native/include/rev/CANMessage.h +++ b/src/main/native/include/rev/CANMessage.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/CANStatus.h b/src/main/native/include/rev/CANStatus.h index 1382e6b..a1895dc 100644 --- a/src/main/native/include/rev/CANStatus.h +++ b/src/main/native/include/rev/CANStatus.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h index 39c7fef..177dc25 100644 --- a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h +++ b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDevice.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h index 78f3f62..564f391 100644 --- a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h +++ b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDeviceThread.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDriver.h b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDriver.h index d431a5b..97cc99f 100644 --- a/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDriver.h +++ b/src/main/native/include/rev/Drivers/CandleWinUSB/CandleWinUSBDriver.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/DriverDeviceThread.h b/src/main/native/include/rev/Drivers/DriverDeviceThread.h index e643b6b..6389157 100644 --- a/src/main/native/include/rev/Drivers/DriverDeviceThread.h +++ b/src/main/native/include/rev/Drivers/DriverDeviceThread.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h b/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h index b507c01..4955efe 100644 --- a/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h +++ b/src/main/native/include/rev/Drivers/SerialPort/SerialDevice.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h b/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h index ade3551..73a4a1f 100644 --- a/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h +++ b/src/main/native/include/rev/Drivers/SerialPort/SerialDeviceThread.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialDriver.h b/src/main/native/include/rev/Drivers/SerialPort/SerialDriver.h index b1f4e18..f560884 100644 --- a/src/main/native/include/rev/Drivers/SerialPort/SerialDriver.h +++ b/src/main/native/include/rev/Drivers/SerialPort/SerialDriver.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/SerialPort/SerialMessage.h b/src/main/native/include/rev/Drivers/SerialPort/SerialMessage.h index 90c404f..4121400 100644 --- a/src/main/native/include/rev/Drivers/SerialPort/SerialMessage.h +++ b/src/main/native/include/rev/Drivers/SerialPort/SerialMessage.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h new file mode 100644 index 0000000..b255c35 --- /dev/null +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2019 - 2020 REV Robotics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of REV Robotics nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +#include "rev/Drivers/SocketCAN/SocketCANThread.h" +#include "rev/CANDevice.h" +#include "rev/CANMessage.h" +#include "rev/CANStatus.h" + +namespace rev { +namespace usb { + +class SocketCANDevice : public CANDevice { +public: + SocketCANDevice() =delete; + SocketCANDevice(const char* port); + virtual ~SocketCANDevice(); + + virtual std::string GetName() const; + virtual std::string GetDescriptor() const; + virtual int GetNumberOfErrors(); + + virtual int GetId() const; + + virtual CANStatus SendCANMessage(const CANMessage& msg, int periodMs) override; + virtual CANStatus ReceiveCANMessage(std::shared_ptr& msg, uint32_t messageID, uint32_t messageMask) override; + virtual CANStatus OpenStreamSession(uint32_t* sessionHandle, CANBridge_CANFilter filter, uint32_t maxSize) override; + virtual CANStatus CloseStreamSession(uint32_t sessionHandle); + virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead); + + virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr); + + virtual bool IsConnected(); +private: + candle_handle m_handle; + SocketCANDeviceThread m_thread; + std::string m_descriptor; + std::string m_name; +}; + +} // namespace usb +} // namespace rev diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDriver.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDriver.h new file mode 100644 index 0000000..dd12edb --- /dev/null +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDriver.h @@ -0,0 +1,51 @@ +/* + * Copyright (c) 2019 - 2020 REV Robotics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of REV Robotics nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include + +#include "rev/CANDriver.h" + +namespace rev { +namespace usb { + +class SocketCANDriver : public CANDriver { +public: + SocketCANDriver() {} + virtual ~SocketCANDriver() override {} + + virtual std::string GetName() const {return "SocketCAN";} + + virtual std::vector GetDevices() override; + virtual std::unique_ptr CreateDeviceFromDescriptor(const char* descriptor) override; +}; + +} // namespace usb +} // namespace rev diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h new file mode 100644 index 0000000..3d0ed56 --- /dev/null +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -0,0 +1,217 @@ +/* + * Copyright (c) 2019 - 2020 REV Robotics + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of REV Robotics nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// TODO: remove me +#include +#include +#include + +#include "rev/CANMessage.h" +#include "rev/CANBridgeUtils.h" +#include "rev/CANStatus.h" +#include "rev/Drivers/DriverDeviceThread.h" + +#include "utils/ThreadUtils.h" + +#include "candlelib/candle.h" + +#include +#include + +namespace rev { +namespace usb { + + +class SocketCANDeviceThread :public DriverDeviceThread { +public: + SocketCANDeviceThread() =delete; + SocketCANDeviceThread(const char* port, long long threadIntervalMs = 1) : DriverDeviceThread(0xe45b5597, threadIntervalMs) + { } + ~SocketCANDeviceThread() + { + } + + void Start() override { + if (m_thread.get() != nullptr && m_thread->joinable()) { + m_thread->join(); + } + + m_thread = std::make_unique(&SocketCANDeviceThread::CandleRun, this); + + // Set to high priority to prevent buffer overflow on the device on high client CPU load + utils::SetThreadPriority(m_thread.get(), utils::ThreadPriority::High); + } + + void OpenStream(uint32_t* handle, CANBridge_CANFilter filter, uint32_t maxSize, CANStatus *status) override { + std::lock_guard lock(m_streamMutex); + + // Create the handle + *handle = m_counter++; + + // Add to the map + m_readStream[*handle] = std::unique_ptr(new CANStreamHandle{filter.messageId, filter.messageMask, maxSize, utils::CircularBuffer>{maxSize}}); + + *status = CANStatus::kOk; + } + + + +private: + candle_handle m_device; + + void ReadMessages(bool &reading) { + candle_frame_t incomingFrame; + + reading = candle_frame_read(m_device, &incomingFrame, 0); + + // Received a new frame, store it + if (reading) { + candle_frametype_t frameType = candle_frame_type(&incomingFrame); + if(frameType == CANDLE_FRAMETYPE_ERROR) { + // Parse error data + if (incomingFrame.can_id & 0x00000040) { + m_statusDetails.busOffCount++; + } + if (incomingFrame.data[1] & 0x02) { + m_statusDetails.txFullCount++; + } + if (incomingFrame.data[1] & 0x10 || incomingFrame.data[1] & 0x04) { + m_statusDetails.receiveErrCount++; + } + if (incomingFrame.data[1] & 0x20 || incomingFrame.data[1] & 0x08 || incomingFrame.data[2] & 0x80 || incomingFrame.data[4]) { + m_statusDetails.transmitErrCount++; + } + } else if(frameType == CANDLE_FRAMETYPE_RECEIVE) { + + auto msg = std::make_shared(candle_frame_id(&incomingFrame), + candle_frame_data(&incomingFrame), + candle_frame_dlc(&incomingFrame), + candle_frame_timestamp_us(&incomingFrame)); + + // Read functions + { + std::lock_guard lock(m_readMutex); + m_readStore[candle_frame_id(&incomingFrame)] = msg; + } + + // Streaming functions + { + std::lock_guard lock(m_streamMutex); + for (auto& stream : m_readStream) { + // Compare current size of the buffer to the max size of the buffer + if (!stream.second->messages.IsFull() + && rev::usb::CANBridge_ProcessMask({stream.second->messageId, stream.second->messageMask}, + msg->GetMessageId())) { + stream.second->messages.Add(msg); + } + } + } + } + + m_threadStatus = CANStatus::kOk; + } + } + + bool WriteMessages(detail::CANThreadSendQueueElement el, std::chrono::steady_clock::time_point now) { + if (el.m_intervalMs == 0 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) { + candle_frame_t frame; + frame.can_dlc = el.m_msg.GetSize(); + // set extended id flag + frame.can_id = el.m_msg.GetMessageId() | 0x80000000; + memcpy(frame.data, el.m_msg.GetData(), frame.can_dlc); + frame.timestamp_us = now.time_since_epoch().count() / 1000; + + // TODO: Feed back an error + if (candle_frame_send(m_device, 0, &frame, false, 20) == false) { + // std::cout << "Failed to send message: " << std::hex << (int)el.m_msg.GetMessageId() << std::dec << " " << candle_error_text(candle_dev_last_error(m_device)) << std::endl; + m_threadStatus = CANStatus::kDeviceWriteError; + m_statusErrCount++; + return false; + } else { + m_threadStatus = CANStatus::kOk; + return true; + } + } + return false; + } + + void CandleRun() { + while (m_threadComplete == false) { + m_threadStatus = CANStatus::kOk; // Start each loop with the status being good. Really only a write issue. + auto sleepTime = std::chrono::steady_clock::now() + std::chrono::milliseconds(m_threadIntervalMs); + + // 1) Handle all received CAN traffic + bool reading = false; + ReadMessages(reading); + + // 2) Schedule CANMessage queue + { + std::lock_guard lock(m_writeMutex); + if (m_sendQueue.size() > 0) { + detail::CANThreadSendQueueElement el = m_sendQueue.front(); + if (el.m_intervalMs == -1) { + m_sendQueue.pop(); + continue; + } + + auto now = std::chrono::steady_clock::now(); + + // Don't pop queue if send fails + if (WriteMessages(el, now)) { + m_sendQueue.pop(); + + // Return to end of queue if repeated + if (el.m_intervalMs > 0 ) { + el.m_prevTimestamp = now; + m_sendQueue.push(el); + } + } + } + } + + // 3) Stall thread + if (!reading && m_sendQueue.empty()) { + std::this_thread::sleep_until(sleepTime); + } + } + + } +}; + +} // namespace usb +} // namespace rev diff --git a/src/main/native/include/utils/CircularBuffer.h b/src/main/native/include/utils/CircularBuffer.h index c9d9cac..ead6629 100644 --- a/src/main/native/include/utils/CircularBuffer.h +++ b/src/main/native/include/utils/CircularBuffer.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/main/native/include/utils/ThreadUtils.h b/src/main/native/include/utils/ThreadUtils.h index f6b517b..2090071 100644 --- a/src/main/native/include/utils/ThreadUtils.h +++ b/src/main/native/include/utils/ThreadUtils.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: diff --git a/src/test/gtest/cpp/main.cpp b/src/test/gtest/cpp/main.cpp index 9c3f256..5f2875f 100644 --- a/src/test/gtest/cpp/main.cpp +++ b/src/test/gtest/cpp/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2019 REV Robotics + * Copyright (c) 2019 - 2020 REV Robotics * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: From 9d6398a61080c5d0a86d3b2da097364ff6cb15aa Mon Sep 17 00:00:00 2001 From: Will Toth Date: Wed, 25 Mar 2020 01:30:51 -0500 Subject: [PATCH 02/14] Simple method to list available drivers --- src/main/native/cpp/CANBridge.cpp | 5 ++- .../cpp/Drivers/SocketCAN/SocketCANDriver.cpp | 36 +++++++++++++++++-- .../rev/Drivers/SocketCAN/SocketCANDevice.h | 3 +- .../rev/Drivers/SocketCAN/SocketCANThread.h | 4 +++ src/test/gtest/cpp/CANMessageTest.cpp | 3 +- src/test/gtest/cpp/main.cpp | 24 +++++++++++++ 6 files changed, 68 insertions(+), 7 deletions(-) diff --git a/src/main/native/cpp/CANBridge.cpp b/src/main/native/cpp/CANBridge.cpp index 33fe438..19dbd09 100644 --- a/src/main/native/cpp/CANBridge.cpp +++ b/src/main/native/cpp/CANBridge.cpp @@ -60,8 +60,11 @@ struct CANBridge_Scan { static const std::vector CANDriverList = { #ifdef _WIN32 new rev::usb::CandleWinUSBDriver(), -#endif new rev::usb::SerialDriver() +#endif +#ifdef __linux__ + //new rev::usb::SocketCANDriver() +#endif }; static std::vector, rev::usb::CANBridge_CANFilter>> CANDeviceList = {}; diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp index d1ce4f6..205aab7 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp @@ -35,6 +35,8 @@ #include #include +#include + namespace rev { namespace usb { @@ -42,19 +44,47 @@ std::vector SocketCANDriver::GetDevices() { std::vector retval; + // TODO: Better way of doing this? + // find canx or vcanx interface names + struct if_nameindex *if_nidxs, *intf; + + if_nidxs = if_nameindex(); + if ( if_nidxs != NULL ) + { + for (intf = if_nidxs; intf->if_index != 0 || intf->if_name != NULL; intf++) + { + char* buf = intf->if_name; + + // Not possible can name, protect later compares + if (strnlen(buf, 4) < 4) { + continue; + } + + // possibly vcanx + if (buf[0] == 'v') { + buf++; + } + + if (strncmp(buf, "can", 3) == 0) { + std::string ifnameStr = std::string(intf->if_name); + retval.push_back( {ifnameStr, ifnameStr, this->GetName()} ); + } + } + + if_freenameindex(if_nidxs); + } + return retval; } std::unique_ptr SocketCANDriver::CreateDeviceFromDescriptor(const char* descriptor) { - uint8_t num_interfaces; - try { return std::make_unique(descriptor); } catch(...) { // do nothing if it failed } - return std::unique_ptr(); + return std::unique_ptr(nullptr); } } // namespace usb diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h index b255c35..f7c5c8e 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -61,8 +61,7 @@ class SocketCANDevice : public CANDevice { virtual bool IsConnected(); private: - candle_handle m_handle; - SocketCANDeviceThread m_thread; + //SocketCANDeviceThread m_thread; std::string m_descriptor; std::string m_name; }; diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h index 3d0ed56..00b3e6c 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -26,6 +26,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#if 0 + #pragma once #include @@ -215,3 +217,5 @@ class SocketCANDeviceThread :public DriverDeviceThread { } // namespace usb } // namespace rev + +#endif diff --git a/src/test/gtest/cpp/CANMessageTest.cpp b/src/test/gtest/cpp/CANMessageTest.cpp index 6280788..ced27ef 100644 --- a/src/test/gtest/cpp/CANMessageTest.cpp +++ b/src/test/gtest/cpp/CANMessageTest.cpp @@ -41,6 +41,7 @@ */ TEST(CANMessageTest, SendAndReceive) { auto handle = CANBridge_Scan(); + int32_t status = 0; int numDevices = CANBridge_NumDevices(handle); @@ -52,7 +53,7 @@ TEST(CANMessageTest, SendAndReceive) { if (numDevices > 0) { std::cout << "Registering single device to HAL" << std::endl; - CANBridge_RegisterDeviceToHAL(CANBridge_GetDeviceDescriptor(handle, 0), 0, 0); + CANBridge_RegisterDeviceToHAL(CANBridge_GetDeviceDescriptor(handle, 0), 0, 0, &status); // auto device = handle->devices[0]; diff --git a/src/test/gtest/cpp/main.cpp b/src/test/gtest/cpp/main.cpp index 5f2875f..cdb64cc 100644 --- a/src/test/gtest/cpp/main.cpp +++ b/src/test/gtest/cpp/main.cpp @@ -32,8 +32,31 @@ #include "gtest/gtest.h" +#include + int main(int argc, char** argv) { + + rev::usb::SocketCANDriver driver; + + auto output = driver.GetDevices(); + + if (output.size() == 0) { + std::cout << "No devices found" << std::endl; + return 1; + } + + for (auto itr = output.begin(); itr != output.end(); itr++) { + std::cout << itr->descriptor << std::endl; + } + + auto device = driver.CreateDeviceFromDescriptor(output[0].descriptor.c_str()); + + std::cout << "Selected device: " << device->GetName() << std::endl; + + return 0; + + #if 0 HAL_Initialize(500, 0); frc::MockDS ds; ds.start(); @@ -43,4 +66,5 @@ int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); int ret = RUN_ALL_TESTS(); return ret; + #endif } From 54e804a03fdab50834c037f8aad4c0230a3d1f73 Mon Sep 17 00:00:00 2001 From: William Toth Date: Tue, 28 Jul 2020 19:19:49 -0500 Subject: [PATCH 03/14] Update README.md --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/README.md b/README.md index 223fce8..4c674e7 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,16 @@ Build and publish steps are done using the Gradle wrapper, `gradlew`. The Gradle The output folders will be generated under `~\releases\maven\`. +## Linux + +This branch is a work in progress. The latest firmware version will work with Linux if the socketCAN and gs_usb drivers are enabled. The following udev rule will work to enable this: + +/etc/udev/rules.d/ + +``` +ACTION=="add", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a30e", RUN+="/sbin/modprobe gs_usb" RUN+="/bin/sh -c 'echo 0483 a30e > /sys/bus/usb/drivers/gs_usb/new_id'" +``` + ## Changelog The SDK Changelog can be viewed with [Changelog.md](Changelog.md). From 505ccaf20d566253c0d47e54adbf0385f278132d Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Mon, 12 Aug 2024 18:01:51 -0500 Subject: [PATCH 04/14] Update SocketCANDevice class implementation Some of the functions and implementations were not readily available nor created. This now resolves those issues. --- .../cpp/Drivers/SocketCAN/SocketCANDriver.cpp | 78 +++++++++++++++++++ .../rev/Drivers/SocketCAN/SocketCANDevice.h | 12 +-- 2 files changed, 84 insertions(+), 6 deletions(-) diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp index 205aab7..8626544 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp @@ -87,6 +87,84 @@ std::unique_ptr SocketCANDriver::CreateDeviceFromDescriptor(const cha return std::unique_ptr(nullptr); } +SocketCANDevice::SocketCANDevice(const char* port) { + // Constructor implementation +} + +SocketCANDevice::~SocketCANDevice() { + // Destructor implementation +} + +std::string SocketCANDevice::GetName() const { + // Implementation + return "Name"; +} + +std::string SocketCANDevice::GetDescriptor() const { + // Implementation + return "Descriptor"; +} + +int SocketCANDevice::GetNumberOfErrors() { + // Implementation + return 0; +} + +int SocketCANDevice::GetId() const { + // Implementation + return 0; +} + +CANStatus SocketCANDevice::SendCANMessage(const CANMessage& msg, int periodMs) { + // Implementation + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::ReceiveCANMessage(std::shared_ptr& msg, uint32_t messageID, uint32_t messageMask) { + // Implementation + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::OpenStreamSession(uint32_t* sessionHandle, CANBridge_CANFilter filter, uint32_t maxSize) { + // Implementation + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::CloseStreamSession(uint32_t sessionHandle) { + // Implementation + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { + // Implementation + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) { + // Implementation + *percentBusUtilization = 0.0f; + *busOff = 0; + *txFull = 0; + *receiveErr = 0; + *transmitErr = 0; + return CANStatus::kOk; +} + +CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) { + // Implementation + *percentBusUtilization = 0.0f; + *busOff = 0; + *txFull = 0; + *receiveErr = 0; + *transmitErr = 0; + *lastErrorTime = 0; + return CANStatus::kOk; +} + +bool SocketCANDevice::IsConnected() { + // Implementation + return true; +} } // namespace usb } // namespace rev diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h index f7c5c8e..a02b9e8 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -41,7 +41,7 @@ namespace usb { class SocketCANDevice : public CANDevice { public: - SocketCANDevice() =delete; + SocketCANDevice() = delete; SocketCANDevice(const char* port); virtual ~SocketCANDevice(); @@ -54,14 +54,14 @@ class SocketCANDevice : public CANDevice { virtual CANStatus SendCANMessage(const CANMessage& msg, int periodMs) override; virtual CANStatus ReceiveCANMessage(std::shared_ptr& msg, uint32_t messageID, uint32_t messageMask) override; virtual CANStatus OpenStreamSession(uint32_t* sessionHandle, CANBridge_CANFilter filter, uint32_t maxSize) override; - virtual CANStatus CloseStreamSession(uint32_t sessionHandle); - virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead); + virtual CANStatus CloseStreamSession(uint32_t sessionHandle) override; + virtual CANStatus ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) override; - virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr); + virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) override; + virtual CANStatus GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) override; - virtual bool IsConnected(); + virtual bool IsConnected() override; private: - //SocketCANDeviceThread m_thread; std::string m_descriptor; std::string m_name; }; From 4fef873dda377e1049f9f54e0f0ee019e47474ff Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 16 Aug 2024 00:51:29 -0500 Subject: [PATCH 05/14] feat: Implement SocketCANDevice class This commit adds the implementation for the SocketCANDevice class, which was previously missing some functions and implementations. Now all required functions are available and the issues have been resolved. --- .../cpp/Drivers/SocketCAN/SocketCANDevice.cpp | 120 ++++++++++++++++++ .../cpp/Drivers/SocketCAN/SocketCANDriver.cpp | 79 ------------ 2 files changed, 120 insertions(+), 79 deletions(-) create mode 100644 src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp new file mode 100644 index 0000000..17fd7af --- /dev/null +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp @@ -0,0 +1,120 @@ +#ifdef __linux__ + +#include "rev/Drivers/SocketCAN/SocketCANDevice.h" + +#include +#include +#include + +namespace rev { +namespace usb { + +SocketCANDevice::SocketCANDevice(const char* port) { + m_thread(port){ + m_descriptor = port; + m_name = "SPARK MAX"; + m_thread.Start(); + } +} + +SocketCANDevice::~SocketCANDevice() { + m_thread.Stop(); +} + +std::string SocketCANDevice::GetName() const { + return m_name; +} + +std::string SocketCANDevice::GetDescriptor() const { + return m_descriptor; +} + +int SocketCANDevice::GetNumberOfErrors() { + return m_thread.GetNumberOfErrors(); +} + +int SocketCANDevice::GetId() const { + m_thread.EnqueueMessage(msg, periodMs); + return m_thread.GetLastThreadError(); +} + +CANStatus SocketCANDevice::SendCANMessage(const CANMessage& msg, int periodMs) { + m_thread.EnqueueMessage(msg, periodMs); + return m_thread.GetLastThreadError(); +} + +CANStatus SocketCANDevice::ReceiveCANMessage(std::shared_ptr& msg, uint32_t messageID, uint32_t messageMask) { + CANStatus status = CANStatus::kTimeout; + + // parse through the keys, find the messges the match, and return it + // The first in the message id, then the messages + std::map> messages; + m_thread.ReceiveMessage(messages); + std::shared_ptr mostRecent; + for (auto& m : messages) { + if ( + CANBridge_ProcessMask({m.second->GetMessageId(), 0}, m.first) + && CANBridge_ProcessMask({messageID, messageMask}, m.first) + && (!mostRecent || m.second->GetTimestampUs() > mostRecent->GetTimestampUs()) + ) { + mostRecent = m.second; + status = CANStatus::kOk; + } + } + + if (status == CANStatus::kOk) { + msg = mostRecent; + status = m_thread.GetLastThreadError(); + } else { + status = CANStatus::kError; + } + + + return status; +} + +CANStatus SocketCANDevice::OpenStreamSession(uint32_t* sessionHandle, CANBridge_CANFilter filter, uint32_t maxSize) { + CANStatus stat = CANStatus::kOk; + m_thread.OpenStream(sessionHandle, filter, maxSize, &stat); + return m_thread.GetLastThreadError(); +} + +CANStatus SocketCANDevice::CloseStreamSession(uint32_t sessionHandle) { + m_thread.CloseStream(sessionHandle); + return m_thread.GetLastThreadError(); +} +} + +CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { + m_thread.ReadStream(sessionHandle, msgs, messagesToRead, messagesRead); + return m_thread.GetLastThreadError(); +} + +CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) { + rev::CANStatus status = m_thread.GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr); + return m_thread.GetLastThreadError; +} + +CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) { + // Implementation + rev::usb::CANStatusDetails details; + *percentBusUtilization = 0.0f; + *busOff = details.busOffCount; + *txFull = details.txFullCount; + *receiveErr = details.retrieveErrCount; + *transmitErr = details.transmitErrCount; + *lastErrorTime = 0; // Not implemented + return m_thread.GetLastThreadError(); +} + +bool SocketCANDevice::IsConnected() { + // Implementation + return true; +} + +} // namespace usb +} // namespace rev + + +#else +#endif \ No newline at end of file diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp index 8626544..e604811 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDriver.cpp @@ -86,85 +86,6 @@ std::unique_ptr SocketCANDriver::CreateDeviceFromDescriptor(const cha } return std::unique_ptr(nullptr); } - -SocketCANDevice::SocketCANDevice(const char* port) { - // Constructor implementation -} - -SocketCANDevice::~SocketCANDevice() { - // Destructor implementation -} - -std::string SocketCANDevice::GetName() const { - // Implementation - return "Name"; -} - -std::string SocketCANDevice::GetDescriptor() const { - // Implementation - return "Descriptor"; -} - -int SocketCANDevice::GetNumberOfErrors() { - // Implementation - return 0; -} - -int SocketCANDevice::GetId() const { - // Implementation - return 0; -} - -CANStatus SocketCANDevice::SendCANMessage(const CANMessage& msg, int periodMs) { - // Implementation - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::ReceiveCANMessage(std::shared_ptr& msg, uint32_t messageID, uint32_t messageMask) { - // Implementation - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::OpenStreamSession(uint32_t* sessionHandle, CANBridge_CANFilter filter, uint32_t maxSize) { - // Implementation - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::CloseStreamSession(uint32_t sessionHandle) { - // Implementation - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { - // Implementation - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) { - // Implementation - *percentBusUtilization = 0.0f; - *busOff = 0; - *txFull = 0; - *receiveErr = 0; - *transmitErr = 0; - return CANStatus::kOk; -} - -CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) { - // Implementation - *percentBusUtilization = 0.0f; - *busOff = 0; - *txFull = 0; - *receiveErr = 0; - *transmitErr = 0; - *lastErrorTime = 0; - return CANStatus::kOk; -} - -bool SocketCANDevice::IsConnected() { - // Implementation - return true; -} } // namespace usb } // namespace rev From aa836160f0beb11c6b20c6888e9fbd3ca5e10d0b Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 16 Aug 2024 01:31:11 -0500 Subject: [PATCH 06/14] Implement SocketCANDeviceThread in SocketCANDevice class This was missing the previous commit. Change compiler options to have SocketCANThread actually compile instead of not. --- src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h | 1 + src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h index a02b9e8..6729dd0 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -62,6 +62,7 @@ class SocketCANDevice : public CANDevice { virtual bool IsConnected() override; private: + SocketCANDeviceThread m_thread; std::string m_descriptor; std::string m_name; }; diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h index 00b3e6c..589ad1a 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -26,7 +26,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#if 0 +#ifdef __linux__ #pragma once From d81cf10f72441c40217f1990e555e4f07e28bc69 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 16 Aug 2024 01:34:00 -0500 Subject: [PATCH 07/14] refactor: Update include path for CanData header in SocketCANThread.h Mirroring SerialDeviceThread.h as it was changed years ago. --- src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp | 1 - src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp index 17fd7af..7bb45e1 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp @@ -83,7 +83,6 @@ CANStatus SocketCANDevice::CloseStreamSession(uint32_t sessionHandle) { m_thread.CloseStream(sessionHandle); return m_thread.GetLastThreadError(); } -} CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { m_thread.ReadStream(sessionHandle, msgs, messagesToRead, messagesRead); diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h index 589ad1a..edeec96 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -52,7 +52,7 @@ #include "candlelib/candle.h" -#include +#include #include namespace rev { From b5928b102b2a5b29ec569a3225b0e30ad12cebfe Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 16 Aug 2024 02:23:34 -0500 Subject: [PATCH 08/14] Fix compiler issues and broken functions Adding more compiler args to prevent other platforms that are not able to compile some platforms to compile files, as it would just error and fail the build. Fixing broken functions such as `SocketCANDevice::GetID()` as it was not returning a string. This is now not implemented but it returns an `int` which is correct for the function. --- src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp | 7 +++---- .../native/include/rev/Drivers/SocketCAN/SocketCANDevice.h | 4 +++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp index 7bb45e1..c2bd604 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp @@ -34,8 +34,7 @@ int SocketCANDevice::GetNumberOfErrors() { } int SocketCANDevice::GetId() const { - m_thread.EnqueueMessage(msg, periodMs); - return m_thread.GetLastThreadError(); + return 0; } CANStatus SocketCANDevice::SendCANMessage(const CANMessage& msg, int periodMs) { @@ -84,13 +83,13 @@ CANStatus SocketCANDevice::CloseStreamSession(uint32_t sessionHandle) { return m_thread.GetLastThreadError(); } -CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { +CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, struct HAL_CANStreamMessage* msgs, uint32_t messagesToRead, uint32_t* messagesRead) { m_thread.ReadStream(sessionHandle, msgs, messagesToRead, messagesRead); return m_thread.GetLastThreadError(); } CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) { - rev::CANStatus status = m_thread.GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr); + rev::usb::CANStatus status = m_thread.GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr); return m_thread.GetLastThreadError; } diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h index 6729dd0..cff1ce8 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -26,7 +26,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifdef __linux__ #include #include @@ -69,3 +69,5 @@ class SocketCANDevice : public CANDevice { } // namespace usb } // namespace rev + +#endif From 115e1110b3a6200f45f6cefb6368546c90647557 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Fri, 16 Aug 2024 16:05:50 -0500 Subject: [PATCH 09/14] Fix SocketCANDevice compilation and broken functions Fix SocketCANDevice from being broken on compiles, from broken constructors to missing includes. --- src/main/native/cpp/CANBridge.cpp | 2 +- .../cpp/Drivers/SocketCAN/SocketCANDevice.cpp | 22 +++++++++---------- .../rev/Drivers/SocketCAN/SocketCANDevice.h | 3 ++- .../rev/Drivers/SocketCAN/SocketCANThread.h | 18 +++++++-------- 4 files changed, 23 insertions(+), 22 deletions(-) diff --git a/src/main/native/cpp/CANBridge.cpp b/src/main/native/cpp/CANBridge.cpp index f1ed311..2d8a9fa 100644 --- a/src/main/native/cpp/CANBridge.cpp +++ b/src/main/native/cpp/CANBridge.cpp @@ -63,7 +63,7 @@ static const std::vector CANDriverList = { new rev::usb::SerialDriver() #endif #ifdef __linux__ - //new rev::usb::SocketCANDriver() + new rev::usb::SocketCANDriver() #endif }; diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp index c2bd604..30c5aa6 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp @@ -6,16 +6,18 @@ #include #include +#include +#include + namespace rev { namespace usb { -SocketCANDevice::SocketCANDevice(const char* port) { - m_thread(port){ +SocketCANDevice::SocketCANDevice(std::string port) : + m_thread(port) { m_descriptor = port; m_name = "SPARK MAX"; m_thread.Start(); } -} SocketCANDevice::~SocketCANDevice() { m_thread.Stop(); @@ -89,22 +91,20 @@ CANStatus SocketCANDevice::ReadStreamSession(uint32_t sessionHandle, struct HAL_ } CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr) { - rev::usb::CANStatus status = m_thread.GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr); - return m_thread.GetLastThreadError; -} - -CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) { - // Implementation rev::usb::CANStatusDetails details; + m_thread.GetCANStatus(&details); *percentBusUtilization = 0.0f; *busOff = details.busOffCount; *txFull = details.txFullCount; - *receiveErr = details.retrieveErrCount; + *receiveErr = details.receiveErrCount; *transmitErr = details.transmitErrCount; - *lastErrorTime = 0; // Not implemented return m_thread.GetLastThreadError(); } +CANStatus SocketCANDevice::GetCANDetailStatus(float* percentBusUtilization, uint32_t* busOff, uint32_t* txFull, uint32_t* receiveErr, uint32_t* transmitErr, uint32_t* lastErrorTime) { + return GetCANDetailStatus(percentBusUtilization, busOff, txFull, receiveErr, transmitErr); +} + bool SocketCANDevice::IsConnected() { // Implementation return true; diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h index cff1ce8..956ba03 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANDevice.h @@ -26,6 +26,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#pragma once #ifdef __linux__ #include @@ -42,7 +43,7 @@ namespace usb { class SocketCANDevice : public CANDevice { public: SocketCANDevice() = delete; - SocketCANDevice(const char* port); + SocketCANDevice(std::string port); virtual ~SocketCANDevice(); virtual std::string GetName() const; diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h index edeec96..63b4d91 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -62,7 +62,7 @@ namespace usb { class SocketCANDeviceThread :public DriverDeviceThread { public: SocketCANDeviceThread() =delete; - SocketCANDeviceThread(const char* port, long long threadIntervalMs = 1) : DriverDeviceThread(0xe45b5597, threadIntervalMs) + SocketCANDeviceThread(std::string port, long long threadIntervalMs = 1) : DriverDeviceThread(0xe45b5597, threadIntervalMs) { } ~SocketCANDeviceThread() { @@ -96,8 +96,8 @@ class SocketCANDeviceThread :public DriverDeviceThread { private: candle_handle m_device; - void ReadMessages(bool &reading) { - candle_frame_t incomingFrame; + void ReadMessages(bool &reading) { + candle_frame_t incomingFrame; reading = candle_frame_read(m_device, &incomingFrame, 0); @@ -147,9 +147,9 @@ class SocketCANDeviceThread :public DriverDeviceThread { m_threadStatus = CANStatus::kOk; } - } + } - bool WriteMessages(detail::CANThreadSendQueueElement el, std::chrono::steady_clock::time_point now) { + bool WriteMessages(detail::CANThreadSendQueueElement el, std::chrono::steady_clock::time_point now) { if (el.m_intervalMs == 0 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) { candle_frame_t frame; frame.can_dlc = el.m_msg.GetSize(); @@ -170,7 +170,7 @@ class SocketCANDeviceThread :public DriverDeviceThread { } } return false; - } + } void CandleRun() { while (m_threadComplete == false) { @@ -187,7 +187,7 @@ class SocketCANDeviceThread :public DriverDeviceThread { if (m_sendQueue.size() > 0) { detail::CANThreadSendQueueElement el = m_sendQueue.front(); if (el.m_intervalMs == -1) { - m_sendQueue.pop(); + m_sendQueue.pop_front(); continue; } @@ -195,12 +195,12 @@ class SocketCANDeviceThread :public DriverDeviceThread { // Don't pop queue if send fails if (WriteMessages(el, now)) { - m_sendQueue.pop(); + m_sendQueue.pop_front(); // Return to end of queue if repeated if (el.m_intervalMs > 0 ) { el.m_prevTimestamp = now; - m_sendQueue.push(el); + m_sendQueue.push_back(el); } } } From bdd47425c905e2912a048361671d793221878c00 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Tue, 20 Aug 2024 18:14:43 -0500 Subject: [PATCH 10/14] Implement SocketCANDriver This is largely untested, however this should bring the functionality of the SocketCAN work somewhat back into play. --- src/main/native/cpp/CANBridge.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/native/cpp/CANBridge.cpp b/src/main/native/cpp/CANBridge.cpp index 2d8a9fa..64e2244 100644 --- a/src/main/native/cpp/CANBridge.cpp +++ b/src/main/native/cpp/CANBridge.cpp @@ -45,6 +45,10 @@ #include "rev/Drivers/SerialPort/SerialDriver.h" +#ifdef __linux__ +#include "rev/Drivers/SocketCAN/SocketCANDriver.h" +#endif + #include #include @@ -63,7 +67,8 @@ static const std::vector CANDriverList = { new rev::usb::SerialDriver() #endif #ifdef __linux__ - new rev::usb::SocketCANDriver() + new rev::usb::SocketCANDriver(), + new rev::usb::SerialDriver() #endif }; @@ -315,3 +320,4 @@ void CANBridge_UnregisterDeviceFromHAL(const char* descriptor) } + From 221ea27370726c784a886fca75a356acb0c8a6fc Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Wed, 21 Aug 2024 16:59:01 -0500 Subject: [PATCH 11/14] Rewrite SocketCANThread header This tears out the majority of what was [candle_dll](https://github.com/HubertD/candle_dll/tree/master) for the Linux driver and implemented the Linux [SocketCAN](https://docs.kernel.org/networking/can.html) driver. Preliminary testing shows that this does functionally work with a single SPARK MAX motor controller, however further testing may be required. --- .../rev/Drivers/SocketCAN/SocketCANThread.h | 157 +++++++++--------- 1 file changed, 80 insertions(+), 77 deletions(-) diff --git a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h index 63b4d91..d643d9f 100644 --- a/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h +++ b/src/main/native/include/rev/Drivers/SocketCAN/SocketCANThread.h @@ -1,31 +1,3 @@ -/* - * Copyright (c) 2019 - 2020 REV Robotics - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * 3. Neither the name of REV Robotics nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - #ifdef __linux__ #pragma once @@ -37,11 +9,13 @@ #include #include #include - -// TODO: remove me -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "rev/CANMessage.h" #include "rev/CANBridgeUtils.h" @@ -50,22 +24,22 @@ #include "utils/ThreadUtils.h" -#include "candlelib/candle.h" - #include #include namespace rev { namespace usb { - -class SocketCANDeviceThread :public DriverDeviceThread { +class SocketCANDeviceThread : public DriverDeviceThread { public: - SocketCANDeviceThread() =delete; - SocketCANDeviceThread(std::string port, long long threadIntervalMs = 1) : DriverDeviceThread(0xe45b5597, threadIntervalMs) - { } - ~SocketCANDeviceThread() - { + SocketCANDeviceThread() = delete; + SocketCANDeviceThread(std::string port, long long threadIntervalMs = 1) + : DriverDeviceThread(0xe45b5597, threadIntervalMs), m_port(port), m_socket(-1) { } + + ~SocketCANDeviceThread() { + if (m_socket != -1) { + close(m_socket); + } } void Start() override { @@ -73,7 +47,13 @@ class SocketCANDeviceThread :public DriverDeviceThread { m_thread->join(); } - m_thread = std::make_unique(&SocketCANDeviceThread::CandleRun, this); + // Initialize SocketCAN + if (!initializeSocketCAN()) { + m_threadStatus = CANStatus::kError; + return; + } + + m_thread = std::make_unique(&SocketCANDeviceThread::SocketRun, this); // Set to high priority to prevent buffer overflow on the device on high client CPU load utils::SetThreadPriority(m_thread.get(), utils::ThreadPriority::High); @@ -91,51 +71,78 @@ class SocketCANDeviceThread :public DriverDeviceThread { *status = CANStatus::kOk; } +private: + std::string m_port; + int m_socket; + + bool initializeSocketCAN() { + struct ifreq ifr; + struct sockaddr_can addr; + + // Create socket + m_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (m_socket < 0) { + perror("SocketCAN socket"); + return false; + } + // Specify CAN interface + std::strncpy(ifr.ifr_name, m_port.c_str(), IFNAMSIZ - 1); + if (ioctl(m_socket, SIOCGIFINDEX, &ifr) < 0) { + perror("SocketCAN ioctl"); + return false; + } + + // Bind the socket to the CAN interface + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(m_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + perror("SocketCAN bind"); + return false; + } + + return true; + } -private: - candle_handle m_device; - void ReadMessages(bool &reading) { - candle_frame_t incomingFrame; - - reading = candle_frame_read(m_device, &incomingFrame, 0); + struct can_frame frame; + int nbytes = read(m_socket, &frame, sizeof(struct can_frame)); - // Received a new frame, store it + reading = (nbytes > 0); if (reading) { - candle_frametype_t frameType = candle_frame_type(&incomingFrame); - if(frameType == CANDLE_FRAMETYPE_ERROR) { + if (frame.can_id & CAN_ERR_FLAG) { + // Handle error frame + m_statusDetails.busOffCount++; + m_threadStatus = CANStatus::kError; // Parse error data - if (incomingFrame.can_id & 0x00000040) { + if (frame.can_id & 0x00000040) { m_statusDetails.busOffCount++; } - if (incomingFrame.data[1] & 0x02) { + if (frame.data[1] & 0x02) { m_statusDetails.txFullCount++; } - if (incomingFrame.data[1] & 0x10 || incomingFrame.data[1] & 0x04) { + if (frame.data[1] & 0x10 || frame.data[1] & 0x04) { m_statusDetails.receiveErrCount++; } - if (incomingFrame.data[1] & 0x20 || incomingFrame.data[1] & 0x08 || incomingFrame.data[2] & 0x80 || incomingFrame.data[4]) { + if (frame.data[1] & 0x20 || frame.data[1] & 0x08 || frame.data[2] & 0x80 || frame.data[4]) { m_statusDetails.transmitErrCount++; } - } else if(frameType == CANDLE_FRAMETYPE_RECEIVE) { - - auto msg = std::make_shared(candle_frame_id(&incomingFrame), - candle_frame_data(&incomingFrame), - candle_frame_dlc(&incomingFrame), - candle_frame_timestamp_us(&incomingFrame)); + } else { + auto msg = std::make_shared(frame.can_id, + frame.data, + frame.can_dlc, + std::chrono::steady_clock::now().time_since_epoch().count() / 1000); // Read functions { std::lock_guard lock(m_readMutex); - m_readStore[candle_frame_id(&incomingFrame)] = msg; + m_readStore[frame.can_id] = msg; } // Streaming functions { std::lock_guard lock(m_streamMutex); for (auto& stream : m_readStream) { - // Compare current size of the buffer to the max size of the buffer if (!stream.second->messages.IsFull() && rev::usb::CANBridge_ProcessMask({stream.second->messageId, stream.second->messageMask}, msg->GetMessageId())) { @@ -150,17 +157,14 @@ class SocketCANDeviceThread :public DriverDeviceThread { } bool WriteMessages(detail::CANThreadSendQueueElement el, std::chrono::steady_clock::time_point now) { - if (el.m_intervalMs == 0 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) { - candle_frame_t frame; + if (el.m_intervalMs <= 1 || (now - el.m_prevTimestamp >= std::chrono::milliseconds(el.m_intervalMs)) ) { + struct can_frame frame; + frame.can_id = el.m_msg.GetMessageId(); frame.can_dlc = el.m_msg.GetSize(); - // set extended id flag - frame.can_id = el.m_msg.GetMessageId() | 0x80000000; - memcpy(frame.data, el.m_msg.GetData(), frame.can_dlc); - frame.timestamp_us = now.time_since_epoch().count() / 1000; - - // TODO: Feed back an error - if (candle_frame_send(m_device, 0, &frame, false, 20) == false) { - // std::cout << "Failed to send message: " << std::hex << (int)el.m_msg.GetMessageId() << std::dec << " " << candle_error_text(candle_dev_last_error(m_device)) << std::endl; + std::memcpy(frame.data, el.m_msg.GetData(), frame.can_dlc); + + int nbytes = write(m_socket, &frame, sizeof(struct can_frame)); + if (nbytes != sizeof(struct can_frame)) { m_threadStatus = CANStatus::kDeviceWriteError; m_statusErrCount++; return false; @@ -172,7 +176,7 @@ class SocketCANDeviceThread :public DriverDeviceThread { return false; } - void CandleRun() { + void SocketRun() { while (m_threadComplete == false) { m_threadStatus = CANStatus::kOk; // Start each loop with the status being good. Really only a write issue. auto sleepTime = std::chrono::steady_clock::now() + std::chrono::milliseconds(m_threadIntervalMs); @@ -211,11 +215,10 @@ class SocketCANDeviceThread :public DriverDeviceThread { std::this_thread::sleep_until(sleepTime); } } - } }; } // namespace usb } // namespace rev -#endif +#endif \ No newline at end of file From 67448aece675f2e9ca05a04e50f27debeaffa1d4 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Wed, 21 Aug 2024 17:23:51 -0500 Subject: [PATCH 12/14] Update README.md Updated section for Linux support. --- README.md | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 185e363..1a0f68c 100644 --- a/README.md +++ b/README.md @@ -65,11 +65,15 @@ The output is placed at `~\releases\maven\release\com\revrobotics\usb\CANBridge- ## Linux -This branch is a work in progress. The latest firmware version will work with Linux if the socketCAN and gs_usb drivers are enabled. The following udev rule will work to enable this: +> [!NOTE] +> +> This branch is a work in progress, testing does show that it does work, however this still might be some issues. If you do run across those issues, please create an issue so we can diagnose. -/etc/udev/rules.d/ +The latest firmware version will work with Linux if the `SocketCAN` and `gs_usb` drivers are enabled. The following udev rule will work to enable this: -``` +Create a new file named and located at: `/etc/udev/rules.d/99-canbridge.rules` + +```text ACTION=="add", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a30e", RUN+="/sbin/modprobe gs_usb" RUN+="/bin/sh -c 'echo 0483 a30e > /sys/bus/usb/drivers/gs_usb/new_id'" ``` From 70d10f36e11bdbf65b5fe3c7d3278d3897229b5b Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Thu, 22 Aug 2024 09:47:43 -0500 Subject: [PATCH 13/14] Remove dangling serial driver Plans to fully delete the serial driver are underway, as this is not supported or even functioning with current firmware. --- src/main/native/cpp/CANBridge.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/native/cpp/CANBridge.cpp b/src/main/native/cpp/CANBridge.cpp index 64e2244..980dddc 100644 --- a/src/main/native/cpp/CANBridge.cpp +++ b/src/main/native/cpp/CANBridge.cpp @@ -64,11 +64,9 @@ struct CANBridge_Scan { static const std::vector CANDriverList = { #ifdef _WIN32 new rev::usb::CandleWinUSBDriver(), - new rev::usb::SerialDriver() #endif #ifdef __linux__ new rev::usb::SocketCANDriver(), - new rev::usb::SerialDriver() #endif }; From 13e38713079087df0d49b2702de61d4b09079e81 Mon Sep 17 00:00:00 2001 From: Garrett Summerfield Date: Thu, 29 Aug 2024 23:52:57 -0500 Subject: [PATCH 14/14] Change hardcoded name for SocketCANDevice SocketCAN devices aren't exclusively, changing it to something more generic. --- src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp index 30c5aa6..cd36981 100644 --- a/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp +++ b/src/main/native/cpp/Drivers/SocketCAN/SocketCANDevice.cpp @@ -15,7 +15,8 @@ namespace usb { SocketCANDevice::SocketCANDevice(std::string port) : m_thread(port) { m_descriptor = port; - m_name = "SPARK MAX"; + // TODO: Get the name of the device, for now just hardcode the name + m_name = "SocketCAN Device"; m_thread.Start(); }