From 87633b83f700f75294ee44142c1d43b977b04db5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Torbj=C3=B8rn=20Ludvigsen?= Date: Thu, 7 Apr 2022 18:13:16 +0200 Subject: [PATCH] Fixes timing issue in get iq callback Without this fix, my can calls wouls always fail for axis1. They didn't fail for axis0. I don't know why. --- Firmware/communication/can/can_simple.cpp | 28 +++++++++++++++-------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/Firmware/communication/can/can_simple.cpp b/Firmware/communication/can/can_simple.cpp index 563e936ef..30bdba552 100644 --- a/Firmware/communication/can/can_simple.cpp +++ b/Firmware/communication/can/can_simple.cpp @@ -328,15 +328,25 @@ bool CANSimple::get_iq_callback(const Axis& axis) { txmsg.isExt = axis.config_.can.is_extended; txmsg.len = 8; - std::optional Idq_setpoint = axis.motor_.current_control_.Idq_setpoint_; - if (!Idq_setpoint.has_value()) { - Idq_setpoint = {0.0f, 0.0f}; - } - - static_assert(sizeof(float) == sizeof(Idq_setpoint->second)); - static_assert(sizeof(float) == sizeof(axis.motor_.current_control_.Iq_measured_)); - can_setSignal(txmsg, Idq_setpoint->second, 0, 32, true); - can_setSignal(txmsg, axis.motor_.current_control_.Iq_measured_, 32, 32, true); + auto& current_control = axis.motor_.current_control_; + auto [dummy, Idq_setpoint] = current_control.Idq_setpoint_.value(); + float const Iq_measured = current_control.Iq_measured_; + + uint32_t floatBytes; + static_assert(sizeof(Idq_setpoint) == sizeof(floatBytes)); + std::memcpy(&floatBytes, &Idq_setpoint, sizeof(floatBytes)); + + txmsg.buf[0] = floatBytes; + txmsg.buf[1] = floatBytes >> 8; + txmsg.buf[2] = floatBytes >> 16; + txmsg.buf[3] = floatBytes >> 24; + + static_assert(sizeof(floatBytes) == sizeof(Iq_measured)); + std::memcpy(&floatBytes, &Iq_measured, sizeof(floatBytes)); + txmsg.buf[4] = floatBytes; + txmsg.buf[5] = floatBytes >> 8; + txmsg.buf[6] = floatBytes >> 16; + txmsg.buf[7] = floatBytes >> 24; return canbus_->send_message(txmsg); }