From 09991d99f0e39151e3056e5246465da6c49941a1 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Wed, 6 Nov 2024 18:22:34 +0000
Subject: [PATCH] autoclear errors on request_axis_state

---
 odrive_node/README.md               |  9 +++++++--
 odrive_node/src/odrive_can_node.cpp | 19 ++++++++++++++++---
 2 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/odrive_node/README.md b/odrive_node/README.md
index 674934c..fe811f9 100644
--- a/odrive_node/README.md
+++ b/odrive_node/README.md
@@ -50,9 +50,14 @@ For information about installation, prerequisites and getting started, check out
 
   This service requires regular heartbeat messages from the ODrive to determine the procedure result and will block until the procedure completes, with a minimum call time of 1 second.
 
-* `/clear_errors`: Clears disarm_reason and procedure_result and re-arms the brake resistor if applicable
+  If the requested state is anything other than IDLE, this sends a `clear_errors` request to the ODrive (see below) before sending the state request.
 
-  If the axis dropped into IDLE because of an error, clearing the errors does not put the axis back into CLOSED_LOOP_CONTROL. To do so, you must request CLOSED_LOOP_CONTROL again explicitly.
+* `/clear_errors`: Manual service call to clear disarm_reason and procedure_result, reset the LED color and re-arm the brake resistor if applicable. See also [`clear_errors()`](https://docs.odriverobotics.com/v/latest/fibre_types/com_odriverobotics_ODrive.html#ODrive.clear_errors).
+
+  This does not affect the axis state.
+
+  If the axis dropped into IDLE because of an error and the intent is to re-enable it, call `/request_axis_state`
+  instead with CLOSED_LOOP_CONTROL, which clears errors automatically.
 
 ### Data Types
 
diff --git a/odrive_node/src/odrive_can_node.cpp b/odrive_node/src/odrive_can_node.cpp
index 3ca3a71..ee32fac 100644
--- a/odrive_node/src/odrive_can_node.cpp
+++ b/odrive_node/src/odrive_can_node.cpp
@@ -194,12 +194,25 @@ void ODriveCanNode::service_clear_errors_callback(const std::shared_ptr<Empty::R
 }
 
 void ODriveCanNode::request_state_callback() {
-    struct can_frame frame;
-    frame.can_id = node_id_ << 5 | CmdId::kSetAxisState;
+    uint32_t axis_state;
     {
         std::unique_lock<std::mutex> guard(axis_state_mutex_);
-        write_le<uint32_t>(axis_state_, frame.data);
+        axis_state = axis_state_;
+    }
+
+    struct can_frame frame;
+
+    if (axis_state != 0) {
+        // Clear errors if requested state is not IDLE
+        frame.can_id = node_id_ << 5 | CmdId::kClearErrors;
+        write_le<uint8_t>(0, frame.data);
+        frame.can_dlc = 1;
+        can_intf_.send_can_frame(frame);
     }
+
+    // Set state
+    frame.can_id = node_id_ << 5 | CmdId::kSetAxisState;
+    write_le<uint32_t>(axis_state, frame.data);
     frame.can_dlc = 4;
     can_intf_.send_can_frame(frame);
 }