From 1bdea53ec8fa0711b56c50b49116078a0eacbffd Mon Sep 17 00:00:00 2001 From: Unknown Date: Wed, 8 Jan 2020 22:11:50 -0500 Subject: [PATCH] Improve testability --- Firmware/Drivers/DRV8301/drv8301.c | 2 +- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/low_level.cpp | 2 +- Firmware/MotorControl/odrive_main.h | 2 +- Firmware/MotorControl/trapTraj.cpp | 2 +- Firmware/MotorControl/{utils.c => utils.cpp} | 4 +- Firmware/MotorControl/{utils.h => utils.hpp} | 2 +- Firmware/Tests/test_can.cpp | 91 ++++++++++ Firmware/Tests/test_runner.cpp | 171 +------------------ Firmware/Tupfile.lua | 6 +- Firmware/communication/ascii_protocol.cpp | 2 +- Firmware/communication/can_helpers.hpp | 88 ++++++++++ Firmware/communication/communication.cpp | 2 +- Firmware/communication/interface_can.cpp | 2 +- Firmware/communication/interface_can.hpp | 81 +-------- Firmware/communication/interface_uart.cpp | 2 +- Firmware/communication/interface_usb.cpp | 2 +- ODrive_Workspace.code-workspace | 3 +- 18 files changed, 200 insertions(+), 266 deletions(-) rename Firmware/MotorControl/{utils.c => utils.cpp} (98%) rename Firmware/MotorControl/{utils.h => utils.hpp} (98%) create mode 100644 Firmware/Tests/test_can.cpp create mode 100644 Firmware/communication/can_helpers.hpp diff --git a/Firmware/Drivers/DRV8301/drv8301.c b/Firmware/Drivers/DRV8301/drv8301.c index 0e90a652..c65cc4d3 100644 --- a/Firmware/Drivers/DRV8301/drv8301.c +++ b/Firmware/Drivers/DRV8301/drv8301.c @@ -45,7 +45,7 @@ // drivers #include "drv8301.h" -#include "utils.h" +#include "utils.hpp" // ************************************************************************** diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 585366e6..b48ff66d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -4,7 +4,7 @@ #include "gpio.h" #include "odrive_main.h" -#include "utils.h" +#include "utils.hpp" #include "communication/interface_can.hpp" Axis::Axis(int axis_num, diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index a3c341f5..908ffb9f 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include #include "odrive_main.h" diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index dec12035..d5832838 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -127,7 +127,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // ODrive specific includes -#include +#include #include #include #include diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index f1e41aa5..2067c35e 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,6 +1,6 @@ #include #include "odrive_main.h" -#include "utils.h" +#include "utils.hpp" // A sign function where input 0 has positive sign (not 0) float sign_hard(float val) { diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.cpp similarity index 98% rename from Firmware/MotorControl/utils.c rename to Firmware/MotorControl/utils.cpp index 3278d614..579a47a7 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.cpp @@ -1,5 +1,5 @@ -#include +#include #include #include #include @@ -151,7 +151,7 @@ float fast_atan2(float y, float x) { // p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg" float horner_fma(float x, const float *coeffs, size_t count) { float result = 0.0f; - for (int idx = 0; idx < count; ++idx) + for (size_t idx = 0; idx < count; ++idx) result = fmaf(result, x, coeffs[idx]); return result; } diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.hpp similarity index 98% rename from Firmware/MotorControl/utils.h rename to Firmware/MotorControl/utils.hpp index 3145c19a..574aa800 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.hpp @@ -58,7 +58,7 @@ extern "C" { #ifdef M_PI #undef M_PI #endif -#define M_PI 3.14159265358979323846f +#define M_PI (3.14159265358979323846f) #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) diff --git a/Firmware/Tests/test_can.cpp b/Firmware/Tests/test_can.cpp new file mode 100644 index 00000000..29f96a79 --- /dev/null +++ b/Firmware/Tests/test_can.cpp @@ -0,0 +1,91 @@ + +#define DOCTEST_IMPLEMENT +#include +#include +#include + +#include "communication/can_helpers.hpp" + +enum InputMode_t { + INPUT_MODE_INACTIVE, + INPUT_MODE_PASSTHROUGH, + INPUT_MODE_VEL_RAMP, + INPUT_MODE_POS_FILTER, + INPUT_MODE_MIX_CHANNELS, + INPUT_MODE_TRAP_TRAJ, +}; + +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; + + auto val = 0x1234; + std::memcpy(rxmsg.buf, &val, sizeof(val)); + + val = can_getSignal(rxmsg, 0, 16, true, 1, 0); + CHECK(val == 0x1234); + + val = can_getSignal(rxmsg, 0, 16, false, 1, 0); + CHECK(val == 0x3412); + + float myFloat = 1234.6789f; + std::memcpy(rxmsg.buf, &myFloat, sizeof(myFloat)); + auto floatVal = can_getSignal(rxmsg, 0, 32, true, 1, 0); + CHECK(floatVal == 1234.6789f); + + can_Message_t msg; + msg.id = 0x00E; + msg.buf[0] = 0x96; + msg.buf[1] = 0x00; + msg.buf[2] = 0x00; + msg.buf[3] = 0x00; + CHECK(can_getSignal(msg, 0, 32, true, 0.01f, 0.0f) == 1.50f); + } + + TEST_CASE("setSignal") { + can_Message_t txmsg; + + can_setSignal(txmsg, 0x1234, 0, 16, true, 1.0f, 0.0f); + CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); + + can_setSignal(txmsg, 0xABCD, 16, 16, true, 1.0f, 0.0f); + CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); + CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); + + can_setSignal(txmsg, 1234.5678f, 32, 32, true, 1.0f, 0.0f); + CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); + CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); + CHECK(can_getSignal(txmsg, 32, 32, true, 1.0f, 0.0f)); + + can_setSignal(txmsg, 0x1234, 0, 16, false, 1.0f, 0.0f); + CHECK(can_getSignal(txmsg, 0, 16, false, 1.0f, 0.0f) == 0x1234); + CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); + CHECK(can_getSignal(txmsg, 32, 32, true, 1.0f, 0.0f)); + + can_setSignal(txmsg, 234981.0f, 12, 32, false, 2.0f, 1.1f); + CHECK(can_getSignal(txmsg, 12, 32, false, 2.0f, 1.1f) == 234981.0f); + } + + TEST_CASE("getSignal enums") { + can_Message_t rxmsg; + rxmsg.buf[0] = INPUT_MODE_MIX_CHANNELS; + rxmsg.buf[1] = INPUT_MODE_PASSTHROUGH; + CHECK(static_cast(can_getSignal(rxmsg, 0, 8, true, 1, 0)) == INPUT_MODE_MIX_CHANNELS); + CHECK(static_cast(can_getSignal(rxmsg, 8, 8, true, 1, 0)) == INPUT_MODE_PASSTHROUGH); + } +} \ No newline at end of file diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp index aa2476f9..e2314477 100644 --- a/Firmware/Tests/test_runner.cpp +++ b/Firmware/Tests/test_runner.cpp @@ -11,181 +11,14 @@ #include +#include + using std::cout; using std::endl; -struct can_Message_t { - uint32_t id = 0x000; // 11-bit max is 0x7ff, 29-bit max is 0x1FFFFFFF - bool isExt = false; - bool rtr = false; - uint8_t len = 8; - uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0}; -}; -struct can_Signal_t { - const uint8_t startBit; - const uint8_t length; - const bool isIntel; - const float factor; - const float offset; -}; -enum InputMode_t { - INPUT_MODE_INACTIVE, - INPUT_MODE_PASSTHROUGH, - INPUT_MODE_VEL_RAMP, - INPUT_MODE_POS_FILTER, - INPUT_MODE_MIX_CHANNELS, - INPUT_MODE_TRAP_TRAJ, -}; -// Fetch a specific signal from the message -template -T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) { - uint64_t tempVal = 0; - uint64_t mask = (1ULL << length) - 1; - - if (isIntel) { - std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); - tempVal = (tempVal >> startBit) & mask; - } else { - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); - tempVal = (tempVal >> (64 - startBit - length)) & mask; - } - - T retVal; - std::memcpy(&retVal, &tempVal, sizeof(T)); - return retVal; -} - -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; -} - -template -void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel) { - uint64_t mask = (1ULL << length) - 1; - uint64_t valAsBits = 0; - std::memcpy(&valAsBits, &val, sizeof(T)); - - if (isIntel) { - uint64_t data = 0; - std::memcpy(&data, msg.buf, sizeof(data)); - - data &= ~(mask << startBit); - data |= valAsBits << startBit; - - std::memcpy(msg.buf, &data, sizeof(data)); - } else { - uint64_t data = 0; - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - std::memcpy(&data, msg.buf, sizeof(data)); - - data &= ~(mask << (64 - startBit - length)); - data |= valAsBits << (64 - startBit - length); - - std::memcpy(msg.buf, &data, sizeof(data)); - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - } -} - -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 = (val - offset) / factor; - can_setSignal(msg, scaledVal, startBit, length, isIntel); -} - -template -float can_getSignal(can_Message_t msg, const can_Signal_t& signal) { - return can_getSignal(msg, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); -} - -template -void can_setSignal(can_Message_t& msg, const T& val, const can_Signal_t& signal) { - can_setSignal(msg, val, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); -} - -TEST_CASE("fake") { - cout << endl; -} - -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; - - auto val = 0x1234; - std::memcpy(rxmsg.buf, &val, sizeof(val)); - - val = can_getSignal(rxmsg, 0, 16, true, 1, 0); - CHECK(val == 0x1234); - - val = can_getSignal(rxmsg, 0, 16, false, 1, 0); - CHECK(val == 0x3412); - - float myFloat = 1234.6789f; - std::memcpy(rxmsg.buf, &myFloat, sizeof(myFloat)); - auto floatVal = can_getSignal(rxmsg, 0, 32, true, 1, 0); - CHECK(floatVal == 1234.6789f); - - can_Message_t msg; - msg.id = 0x00E; - msg.buf[0] = 0x96; - msg.buf[1] = 0x00; - msg.buf[2] = 0x00; - msg.buf[3] = 0x00; - CHECK(can_getSignal(msg, 0, 32, true, 0.01f, 0.0f) == 1.50f); - } - - TEST_CASE("setSignal") { - can_Message_t txmsg; - - can_setSignal(txmsg, 0x1234, 0, 16, true, 1.0f, 0.0f); - CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); - - can_setSignal(txmsg, 0xABCD, 16, 16, true, 1.0f, 0.0f); - CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); - CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); - - can_setSignal(txmsg, 1234.5678f, 32, 32, true, 1.0f, 0.0f); - CHECK(can_getSignal(txmsg, 0, 16, true, 1.0f, 0.0f) == 0x1234); - CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); - CHECK(can_getSignal(txmsg, 32, 32, true, 1.0f, 0.0f)); - - can_setSignal(txmsg, 0x1234, 0, 16, false, 1.0f, 0.0f); - CHECK(can_getSignal(txmsg, 0, 16, false, 1.0f, 0.0f) == 0x1234); - CHECK(can_getSignal(txmsg, 16, 16, true, 1.0f, 0.0f) == 0xABCD); - CHECK(can_getSignal(txmsg, 32, 32, true, 1.0f, 0.0f)); - - can_setSignal(txmsg, 234981.0f, 12, 32, false, 2.0f, 1.1f); - CHECK(can_getSignal(txmsg, 12, 32, false, 2.0f, 1.1f) == 234981.0f); - } - - TEST_CASE("getSignal enums") { - can_Message_t rxmsg; - rxmsg.buf[0] = INPUT_MODE_MIX_CHANNELS; - rxmsg.buf[1] = INPUT_MODE_PASSTHROUGH; - CHECK(static_cast(can_getSignal(rxmsg, 0, 8, true, 1, 0)) == INPUT_MODE_MIX_CHANNELS); - CHECK(static_cast(can_getSignal(rxmsg, 8, 8, true, 1, 0)) == INPUT_MODE_PASSTHROUGH); - } -} TEST_SUITE("delta_enc") { // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271 diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index d329e4e7..63ca9117 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -157,7 +157,7 @@ build{ packages={'stm_platform'}, sources={ 'Drivers/DRV8301/drv8301.c', - 'MotorControl/utils.c', + 'MotorControl/utils.cpp', 'MotorControl/arm_sin_f32.c', 'MotorControl/arm_cos_f32.c', 'MotorControl/low_level.cpp', @@ -190,7 +190,7 @@ build{ } if tup.getconfig('DOCTEST') == 'true' then - TEST_INCLUDES = '-IC:/Tools/doctest/doctest' - tup.frule{inputs='Tests/test_runner.cpp', command='g++ -O3 -std=gnu++17 '..TEST_INCLUDES..' %f -o %o', outputs='Tests/test_runner.exe'} + TEST_INCLUDES = '-I. -I./MotorControl -I./fibre/cpp/include -I./Drivers/DRV8301 -IC:/Tools/doctest/doctest' + tup.frule{inputs='Tests/*.cpp', command='g++ -O3 -std=gnu++17 '..TEST_INCLUDES..' %f -o %o', outputs='Tests/test_runner.exe'} tup.frule{inputs='Tests/test_runner.exe', command='%f'} end \ No newline at end of file diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 3a617649..54e6cccc 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -11,7 +11,7 @@ #include "../build/version.h" // autogenerated based on Git state #include "communication.h" #include "ascii_protocol.hpp" -#include +#include #include /* Private macros ------------------------------------------------------------*/ diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can_helpers.hpp new file mode 100644 index 00000000..b5e40385 --- /dev/null +++ b/Firmware/communication/can_helpers.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include + +struct can_Message_t { + uint32_t id = 0x000; // 11-bit max is 0x7ff, 29-bit max is 0x1FFFFFFF + bool isExt = false; + bool rtr = false; + uint8_t len = 8; + uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0}; +} ; + +struct can_Signal_t { + const uint8_t startBit; + const uint8_t length; + const bool isIntel; + const float factor; + const float offset; +}; + + +#include +template +T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) { + uint64_t tempVal = 0; + uint64_t mask = (1ULL << length) - 1; + + if (isIntel) { + std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); + tempVal = (tempVal >> startBit) & mask; + } else { + std::reverse(std::begin(msg.buf), std::end(msg.buf)); + std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); + tempVal = (tempVal >> (64 - startBit - length)) & mask; + } + + T retVal; + std::memcpy(&retVal, &tempVal, sizeof(T)); + return retVal; +} + +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; +} + +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 = (val - offset) / factor; + uint64_t valAsBits = 0; + std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal)); + + uint64_t mask = (1ULL << length) - 1; + + if (isIntel) { + uint64_t data = 0; + std::memcpy(&data, msg.buf, sizeof(data)); + + data &= ~(mask << startBit); + data |= valAsBits << startBit; + + std::memcpy(msg.buf, &data, sizeof(data)); + } else { + uint64_t data = 0; + std::reverse(std::begin(msg.buf), std::end(msg.buf)); + std::memcpy(&data, msg.buf, sizeof(data)); + + data &= ~(mask << (64 - startBit - length)); + data |= valAsBits << (64 - startBit - length); + + std::memcpy(msg.buf, &data, sizeof(data)); + std::reverse(std::begin(msg.buf), std::end(msg.buf)); + } +} + +template +float can_getSignal(can_Message_t msg, const can_Signal_t& signal) { + return can_getSignal(msg, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); +} + +template +void can_setSignal(can_Message_t& msg, const T& val, const can_Signal_t& signal) { + can_setSignal(msg, val, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); +} \ No newline at end of file diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index a5cd4bcb..f90bbbb1 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -10,7 +10,7 @@ #include "odrive_main.h" #include "freertos_vars.h" -#include "utils.h" +#include "utils.hpp" #include "../build/version.h" // autogenerated based on Git state diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp index 00637597..bf7f9ff0 100644 --- a/Firmware/communication/interface_can.cpp +++ b/Firmware/communication/interface_can.cpp @@ -2,7 +2,7 @@ #include "fibre/crc.hpp" #include "freertos_vars.h" -#include "utils.h" +#include "utils.hpp" #include #include diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 7a1e9c9a..ffb29bb7 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -5,26 +5,11 @@ #include #include "fibre/protocol.hpp" #include "odrive_main.h" +#include "can_helpers.hpp" #define CAN_CLK_HZ (42000000) #define CAN_CLK_MHZ (42) -struct can_Message_t { - uint32_t id = 0x000; // 11-bit max is 0x7ff, 29-bit max is 0x1FFFFFFF - bool isExt = false; - bool rtr = false; - uint8_t len = 8; - uint8_t buf[8] = {0, 0, 0, 0, 0, 0, 0, 0}; -} ; - -struct can_Signal_t { - const uint8_t startBit; - const uint8_t length; - const bool isIntel; - const float factor; - const float offset; -}; - // Anonymous enum for defining the most common CAN baud rates enum { CAN_BAUD_125K = 125000, @@ -87,70 +72,6 @@ class ODriveCAN { void set_baud_rate(uint32_t baudRate); }; -#include -template -T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) { - uint64_t tempVal = 0; - uint64_t mask = (1ULL << length) - 1; - - if (isIntel) { - std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); - tempVal = (tempVal >> startBit) & mask; - } else { - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - std::memcpy(&tempVal, msg.buf, sizeof(tempVal)); - tempVal = (tempVal >> (64 - startBit - length)) & mask; - } - - T retVal; - std::memcpy(&retVal, &tempVal, sizeof(T)); - return retVal; -} - -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; -} - -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 = (val - offset) / factor; - uint64_t valAsBits = 0; - std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal)); - - uint64_t mask = (1ULL << length) - 1; - - if (isIntel) { - uint64_t data = 0; - std::memcpy(&data, msg.buf, sizeof(data)); - - data &= ~(mask << startBit); - data |= valAsBits << startBit; - - std::memcpy(msg.buf, &data, sizeof(data)); - } else { - uint64_t data = 0; - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - std::memcpy(&data, msg.buf, sizeof(data)); - - data &= ~(mask << (64 - startBit - length)); - data |= valAsBits << (64 - startBit - length); - - std::memcpy(msg.buf, &data, sizeof(data)); - std::reverse(std::begin(msg.buf), std::end(msg.buf)); - } -} - -template -float can_getSignal(can_Message_t msg, const can_Signal_t& signal) { - return can_getSignal(msg, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); -} - -template -void can_setSignal(can_Message_t& msg, const T& val, const can_Signal_t& signal) { - can_setSignal(msg, val, signal.startBit, signal.length, signal.isIntel, signal.factor, signal.offset); -} DEFINE_ENUM_FLAG_OPERATORS(ODriveCAN::Error_t) diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index f1bb5e0d..ae08d90c 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -3,7 +3,7 @@ #include "ascii_protocol.hpp" -#include +#include #include #include diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index c52fe5ca..59ee5f2a 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -2,7 +2,7 @@ #include "interface_usb.h" #include "ascii_protocol.hpp" -#include +#include #include #include diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index cf197f8d..8c1fe5c3 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -61,7 +61,8 @@ "fstream": "cpp", "iomanip": "cpp", "optional": "cpp", - "sstream": "cpp" + "sstream": "cpp", + "utils.h": "c" } } }