diff --git a/Firmware/Tests/test_can.cpp b/Firmware/Tests/test_can.cpp index 1f9ffc54..84ba9690 100644 --- a/Firmware/Tests/test_can.cpp +++ b/Firmware/Tests/test_can.cpp @@ -3,7 +3,7 @@ #include #include -#include "communication/can/can_helpers.hpp" +#include "../interfaces/can_helpers.hpp" enum InputMode { INPUT_MODE_INACTIVE, @@ -15,21 +15,6 @@ enum InputMode { }; TEST_SUITE("CAN Functions") { - TEST_CASE("reverse") { - can_Message_t rxmsg; - rxmsg.id = 0x000; - rxmsg.isExt = false; - rxmsg.len = 8; - - rxmsg.buf[0] = 0x12; - rxmsg.buf[1] = 0x34; - - std::reverse(std::begin(rxmsg.buf), std::end(rxmsg.buf)); - CHECK(rxmsg.buf[0] == 0x00); - CHECK(rxmsg.buf[6] == 0x34); - CHECK(rxmsg.buf[7] == 0x12); - } - TEST_CASE("getSignal") { can_Message_t rxmsg; @@ -61,6 +46,10 @@ TEST_SUITE("CAN Functions") { const auto bigVal = 0x123ULL << 28ULL; std::memcpy(rxmsg.buf, &bigVal, sizeof(bigVal)); CHECK(can_getSignal(rxmsg, 28, 12, true) == 0x123ULL); + + const auto myVal = 0xDEADBEEF; + std::memcpy(&rxmsg.buf[15], &myVal, sizeof(myVal)); + CHECK(can_getSignal(rxmsg, 120, 32, true) == 0xDEADBEEF); } TEST_CASE("setSignal") { diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp index 7e738f80..10cef4b1 100644 --- a/Firmware/Tests/test_runner.cpp +++ b/Firmware/Tests/test_runner.cpp @@ -11,7 +11,6 @@ #include -#include using std::cout; using std::endl; diff --git a/Firmware/interfaces/can_helpers.hpp b/Firmware/interfaces/can_helpers.hpp index 09ae3d52..8ef22dca 100644 --- a/Firmware/interfaces/can_helpers.hpp +++ b/Firmware/interfaces/can_helpers.hpp @@ -1,6 +1,7 @@ #pragma once #include + #include #include #include @@ -30,7 +31,7 @@ struct can_Message_t { * `nominal_baud_rate`. Must be false if `fd_frame` is false. */ bool bit_rate_switching = false; - + /** * Controls the FDF bit (aka r0 in Classical CAN). Must be false on * interfaces that don't support CAN FD. @@ -39,7 +40,7 @@ struct can_Message_t { uint8_t len = 8; uint8_t buf[64] = {0}; -} ; +}; struct can_Signal_t { const uint8_t startBit; @@ -55,17 +56,17 @@ struct can_Cyclic_t { }; #pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wstrict-aliasing" // Make sure to check these functions on your system +#pragma GCC diagnostic ignored "-Wstrict-aliasing" // Make sure to check these functions on your system template constexpr T can_getSignal(const can_Message_t& msg, const uint8_t startBit, const uint8_t length, const bool isIntel) { uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; - uint64_t tempVal = *(reinterpret_cast(msg.buf)); + uint64_t tempVal = *(reinterpret_cast(&msg.buf[startBit / 8])); if (isIntel) { - tempVal = (tempVal >> startBit) & mask; + tempVal = (tempVal >> startBit % 8) & mask; } else { - tempVal = __builtin_bswap64 (tempVal); - tempVal = (tempVal >> (64 - startBit - length)) & mask; + tempVal = __builtin_bswap64(tempVal); + tempVal = (tempVal >> (64 - (startBit % 8) - length)) & mask; } return *(reinterpret_cast(&tempVal)); @@ -74,43 +75,39 @@ constexpr T can_getSignal(const can_Message_t& msg, const uint8_t startBit, cons template constexpr void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel) { union aliastype { - aliastype() : valAsBits(0){} + aliastype() : valAsBits(0) {} T tempVal; uint64_t valAsBits; }; const uint64_t mask = length < 64 ? (1ULL << length) - 1ULL : -1ULL; - uint64_t data = *(reinterpret_cast(msg.buf)); + const uint8_t shift = isIntel ? (startBit % 8) : (64 - startBit % 8) - length; aliastype valAlias; valAlias.tempVal = val; valAlias.valAsBits &= mask; + uint64_t data = *(reinterpret_cast(&msg.buf[startBit / 8])); if (isIntel) { - data &= ~(mask << startBit); - data |= valAlias.valAsBits << startBit; - - *(reinterpret_cast(msg.buf)) = data; + data &= ~(mask << shift); + data |= valAlias.valAsBits << shift; } else { - data = __builtin_bswap64 (data); - - data &= ~(mask << (64 - startBit - length)); - data |= valAlias.valAsBits << (64 - startBit - length); - - data = __builtin_bswap64 (data); - *(reinterpret_cast(msg.buf)) = data; - + data = __builtin_bswap64(data); + data &= ~(mask << shift); + data |= valAlias.valAsBits << shift; + data = __builtin_bswap64(data); } + *(reinterpret_cast(&msg.buf[startBit / 8])) = data; } #pragma GCC diagnostic pop -template +template void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) { T scaledVal = static_cast((val - offset) / factor); can_setSignal(msg, scaledVal, startBit, length, isIntel); } -template +template float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) { T retVal = can_getSignal(msg, startBit, length, isIntel); return (retVal * factor) + offset; diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 34f5b335..6239927a 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -101,7 +101,27 @@ "readerwriter": "cpp", "charconv": "cpp", "image": "cpp", - "imageutils": "cpp" + "imageutils": "cpp", + "ios": "cpp", + "locale": "cpp", + "queue": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xmemory0": "cpp", + "xstddef": "cpp", + "xstring": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp" } } }