Await acks for write SDO, filter endpoint IDs

src/ODriveCAN.h

@@ -323,6 +323,12 @@ class ODriveCAN {
             return T{};
         }
 
+        // Ignore a TxSdo that echoes a different endpoint than we requested.
+        uint16_t resp_endpoint = (uint16_t)buffer_[1] | ((uint16_t)buffer_[2] << 8);
+        if (resp_endpoint != endpoint_id) {
+            return T{};
+        }
+
         T ret{};
         memcpy(&ret, &buffer_[4], sizeof(T));
         return ret;
@@ -334,24 +340,43 @@ class ODriveCAN {
      * @tparam T Type of the value from flat_endpoints.json
      * @param endpoint_id Unique ID of endpoint from flat_endpoints.json
      * @param value value to write to the endpoint
+     * @param timeout_ms Time to wait for the ODrive's TxSdo acknowledgment.
+     *                   Pass 0 to send without waiting (fire-and-forget).
      *
-     * This function returns immediately and does not check if the ODrive
-     * received the CAN message.
+     * @return true if the ODrive acknowledged the write (or timeout_ms == 0),
+     *         false if no matching TxSdo was received before the timeout.
+     *
+     * The ODrive replies with a TxSdo message in response to any RxSdo,
+     * including writes, so this blocks until that acknowledgment is received
+     * or the timeout is reached.
      */
     template<typename T>
-    bool setEndpoint(uint16_t endpoint_id, T value) {
+    bool setEndpoint(uint16_t endpoint_id, T value, uint16_t timeout_ms = 10) {
         uint8_t data[8] = {};
         data[0] = 1; // Opcode write
 
-        // Endpoint
-        data[1] = endpoint_id & 0xFF;
-        data[2] = (endpoint_id >> 8) & 0xFF;
+        // Little-endian endpoint
+        data[1] = (uint8_t)(endpoint_id);
+        data[2] = (uint8_t)(endpoint_id >> 8);
 
         // Value to write
         memcpy(&data[4], &value, sizeof(T));
 
+        requested_msg_id_ = 0x005; // Await TxSdo acknowledgment
         can_intf_.sendMsg((node_id_ << ODriveCAN::kNodeIdShift) | 0x004, 8, data);
-        return true;
+
+        if (timeout_ms == 0) {
+            requested_msg_id_ = REQUEST_PENDING; // fire-and-forget, don't wait
+            return true;
+        }
+
+        if (!awaitMsg(timeout_ms)) {
+            return false;
+        }
+
+        // Confirm the acknowledgment echoes the endpoint we wrote to.
+        uint16_t acked_endpoint = (uint16_t)buffer_[1] | ((uint16_t)buffer_[2] << 8);
+        return acked_endpoint == endpoint_id;
     }
 
 private:

src/ODriveHardwareCAN.hpp

@@ -9,6 +9,9 @@
 // Must be defined by the application
 void onCanMessage(const CanMsg& msg);
 
+// Forward declaration; defined below and used by sendMsg().
+static void pumpEvents(HardwareCAN& intf, int max_events = 100);
+
 /**
  * @brief Sends a CAN message over the specified platform-specific interface.
  *
@@ -26,12 +29,19 @@ static bool sendMsg(HardwareCAN& can_intf, uint32_t id, uint8_t length, const ui
     // Note: Arduino_CAN does not support the RTR bit. The ODrive interprets
     // zero-length packets the same as RTR=1, but it creates the possibility of
     // collisions.
+
+    // Flush pending RX messages before writing. On polling-based platforms (e.g.
+    // Arduino Uno R4) the single TX mailbox may report busy if the CAN
+    // controller hasn't had a chance to process events. This mirrors the
+    // can_intf.events() call in the FlexCAN (Teensy) adapter.
+    pumpEvents(can_intf);
+
     CanMsg msg{
         (id & 0x80000000) ? CanExtendedId(id) : CanStandardId(id),
         length,
         data,
     };
-    return can_intf.write(msg) >= 0;
+    return can_intf.write(msg) > 0;
 }
 
 /**
@@ -56,7 +66,7 @@ static void onReceive(const CanMsg& msg, ODriveCAN& odrive) {
  * @param max_events: The maximum number of events to process. This prevents
  *        an infinite loop if messages come at a high rate.
  */
-static void pumpEvents(HardwareCAN& intf, int max_events = 100) {
+static void pumpEvents(HardwareCAN& intf, int max_events) {
     // max_events prevents an infinite loop if messages come at a high rate
     while (intf.available() && max_events--) {
         onCanMessage(intf.read());