Merge pull request #33 from odriverobotics/autoclear

Autoclear errors on request_axis_state

odrive_node/README.md

@@ -51,9 +51,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
 

odrive_node/src/odrive_can_node.cpp

@@ -205,12 +205,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);
 }