From c88f8a4002722cd88a69cf8263992c2c089c182d Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Fri, 28 Mar 2025 18:37:24 +0100
Subject: [PATCH] Check requested message before trying callbacks
---
src/ODriveCAN.cpp | 13 +++++++++----
src/ODriveCAN.h | 2 +-
2 files changed, 10 insertions(+), 5 deletions(-)
diff --git a/src/ODriveCAN.cpp b/src/ODriveCAN.cpp
index d84ffe6..dfb2d3f 100644
--- a/src/ODriveCAN.cpp
+++ b/src/ODriveCAN.cpp
@@ -145,8 +145,17 @@ void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) {
Serial.print(msg.data[byte_index--], HEX);
Serial.println(F(""));
#endif // DEBUG
+ // Check that the message is meant for this node.
if (node_id_ != (id >> ODriveCAN::kNodeIdShift))
return;
+ // If the message is requested, copy it in the request buffer and exit.
+ if ((id & ODriveCAN::kCmdIdBits) == requested_msg_id_) {
+ memcpy(buffer_, data, length);
+ requested_msg_id_ = REQUEST_PENDING;
+ return;
+ };
+ // Check if any of the registered callback handlers apply. Useful for cyclic
+ // messages.
switch (id & ODriveCAN::kCmdIdBits) {
case Get_Encoder_Estimates_msg_t::cmd_id: {
Get_Encoder_Estimates_msg_t estimates;
@@ -178,10 +187,6 @@ void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) {
Serial.print(F("waiting for: 0x"));
Serial.println(requested_msg_id_, HEX);
#endif // DEBUG
- if ((id & ODriveCAN::kCmdIdBits) != requested_msg_id_)
- return;
- memcpy(buffer_, data, length);
- requested_msg_id_ = REQUEST_PENDING;
}
}
}
diff --git a/src/ODriveCAN.h b/src/ODriveCAN.h
index 8b6d06d..06b780a 100644
--- a/src/ODriveCAN.h
+++ b/src/ODriveCAN.h
@@ -161,7 +161,7 @@ class ODriveCAN {
bool getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms = 10);
/**
- * @brief Requests encoder feedback data. May trigger onFeedback callback if it's registered
+ * @brief Requests encoder feedback data.
*
* This function will block and wait for up to timeout_ms (default 10msec) for ODrive to reply
*/