From 7d92f5b8622055b24b849594ddaf4e7c2229e6c4 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Tue, 28 May 2019 23:04:06 +0200 Subject: [PATCH] Create new functions without float or offset --- Firmware/Tests/test_runner.cpp | 25 +++++++++++++++++------- Firmware/communication/can_simple.cpp | 18 ++++++++--------- Firmware/communication/interface_can.hpp | 8 +++++++- 3 files changed, 34 insertions(+), 17 deletions(-) diff --git a/Firmware/Tests/test_runner.cpp b/Firmware/Tests/test_runner.cpp index b7f7f7c1..fa3995da 100644 --- a/Firmware/Tests/test_runner.cpp +++ b/Firmware/Tests/test_runner.cpp @@ -41,7 +41,7 @@ enum InputMode_t { // Fetch a specific signal from the message 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 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; @@ -56,16 +56,21 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len 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)); - +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; @@ -88,6 +93,12 @@ void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, con } } +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); diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index d4d68aa2..a0ecf937 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -185,7 +185,7 @@ void CANSimple::set_axis_nodeid_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_axis_requested_state_callback(Axis* axis, can_Message_t& msg) { - axis->requested_state_ = static_cast(can_getSignal(msg, 0, 16, true, 1, 0)); + axis->requested_state_ = static_cast(can_getSignal(msg, 0, 16, true)); } void CANSimple::set_axis_startup_config_callback(Axis* axis, can_Message_t& msg) { // Not Implemented @@ -276,7 +276,7 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true, 1, 0); + axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); axis->controller_.input_current_ = can_getSignal(msg, 48, 16, true, 0.01f, 0); axis->controller_.input_pos_updated(); @@ -292,12 +292,12 @@ void CANSimple::set_input_current_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_controller_modes_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.config_.control_mode = static_cast(can_getSignal(msg, 0, 32, true, 1, 0)); - axis->controller_.config_.input_mode = static_cast(can_getSignal(msg, 32, 32, true, 1, 0)); + axis->controller_.config_.control_mode = static_cast(can_getSignal(msg, 0, 32, true)); + axis->controller_.config_.input_mode = static_cast(can_getSignal(msg, 32, 32, true)); } void CANSimple::set_vel_limit_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.config_.vel_limit = can_getSignal(msg, 0, 32, true, 1, 0); + axis->controller_.config_.vel_limit = can_getSignal(msg, 0, 32, true); } void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) { @@ -305,16 +305,16 @@ void CANSimple::start_anticogging_callback(Axis* axis, can_Message_t& msg) { } void CANSimple::set_traj_vel_limit_callback(Axis* axis, can_Message_t& msg) { - axis->trap_.config_.vel_limit = can_getSignal(msg, 0, 32, true, 1, 0); + axis->trap_.config_.vel_limit = can_getSignal(msg, 0, 32, true); } void CANSimple::set_traj_accel_limits_callback(Axis* axis, can_Message_t& msg) { - axis->trap_.config_.accel_limit = can_getSignal(msg, 0, 32, true, 1, 0); - axis->trap_.config_.decel_limit = can_getSignal(msg, 32, 32, true, 1, 0); + axis->trap_.config_.accel_limit = can_getSignal(msg, 0, 32, true); + axis->trap_.config_.decel_limit = can_getSignal(msg, 32, 32, true); } void CANSimple::set_traj_A_per_css_callback(Axis* axis, can_Message_t& msg) { - axis->controller_.config_.inertia = can_getSignal(msg, 0, 32, true, 1, 0); + axis->controller_.config_.inertia = can_getSignal(msg, 0, 32, true); } void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) { diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp index 7b8ad661..dcd06232 100644 --- a/Firmware/communication/interface_can.hpp +++ b/Firmware/communication/interface_can.hpp @@ -88,7 +88,7 @@ class ODriveCAN { #include 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 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; @@ -103,6 +103,12 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len 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; }