PR review changes part 2

include/can/core/ids.hpp

@@ -160,6 +160,7 @@ enum class ErrorCode {
 
 /** Error Severity levels. */
 enum class ErrorSeverity {
+    none = 0x0,
     warning = 0x1,
     recoverable = 0x2,
     unrecoverable = 0x3,

include/can/core/message_handlers/motion.hpp

@@ -18,7 +18,8 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     MotorDriverErrorEncountered, ResetMotorDriverErrorHandling>;
+                     MotorDriverErrorEncountered,
+                     ResetMotorDriverErrorHandling>;
 
     MotionHandler(MotionTaskClient &motion_client)
         : motion_client{motion_client} {}

include/motor-control/core/brushed_motor/brushed_motion_controller.hpp

@@ -101,13 +101,15 @@ class MotionController {
         enabled = false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         // if we're gripping something we need to flag this so we don't drop it
         if (!hardware.get_stay_enabled()) {
             if (hardware.is_timer_interrupt_running()) {
-                hardware.request_cancel(static_cast<uint8_t>(error_severity));
+                hardware.request_cancel(error_severity, error_code);
             }
         }
     }

include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp

@@ -201,7 +201,7 @@ class BrushedMotorInterruptHandler {
         } else if (estop_triggered()) {
             in_estop = true;
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
-        } else if (hardware.has_cancel_request() != 0U) {
+        } else if (hardware.has_cancel_request().code != 0U) {
             if (!hardware.get_stay_enabled()) {
                 hardware.set_motor_state(BrushedMotorState::UNHOMED);
             }

include/motor-control/core/motor_hardware_interface.hpp

@@ -72,6 +72,12 @@ class UsageEEpromConfig {
     size_t num_keys = 0;
 };
 
+// std::optional usage? See HardwareConfig struct
+struct __attribute__((packed)) CancelRequest {
+    uint8_t severity;
+    uint8_t code;
+};
+
 class MotorHardwareIface {
   public:
     MotorHardwareIface() = default;
@@ -98,8 +104,9 @@ class MotorHardwareIface {
     virtual void enable_encoder() = 0;
     virtual void disable_encoder() = 0;
 
-    virtual auto has_cancel_request() -> uint8_t = 0;
-    virtual void request_cancel(uint8_t error_severity) = 0;
+    virtual auto has_cancel_request() -> CancelRequest = 0;
+    virtual void request_cancel(can::ids::ErrorSeverity error_severity,
+                                can::ids::ErrorCode error_code) = 0;
     virtual void clear_cancel_request() = 0;
     virtual auto get_usage_eeprom_config() -> const UsageEEpromConfig& = 0;
 

include/motor-control/core/stepper_motor/motion_controller.hpp

@@ -107,17 +107,17 @@ class MotionController {
         return update_queue.try_write(can_msg);
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel(static_cast<uint8_t>(error_severity));
+            hardware.request_cancel(error_severity, error_code);
         }
     }
 
-    void clear_cancel_request() {
-        hardware.clear_cancel_request();
-    }
+    void clear_cancel_request() { hardware.clear_cancel_request(); }
 
     auto is_timer_interrupt_running() -> bool {
         return hardware.is_timer_interrupt_running();
@@ -268,14 +268,16 @@ class PipetteMotionController {
         return false;
     }
 
-    void stop(can::ids::ErrorSeverity error_severity =
-                  can::ids::ErrorSeverity::warning) {
+    void stop(
+        can::ids::ErrorSeverity error_severity =
+            can::ids::ErrorSeverity::warning,
+        can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         // if the timer interrupt is running, cancel it. if it isn't running,
         // don't submit a cancel because then the cancel won't be read until
         // the timer starts the next time.
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel(static_cast<uint8_t>(error_severity));
+            hardware.request_cancel(error_severity, error_code);
         }
     }
 

include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp

@@ -187,7 +187,8 @@ class MotorInterruptHandler {
 
     void run_interrupt() {
         // handle various error states
-        uint8_t has_cancel_request = hardware.has_cancel_request();
+        motor_hardware::CancelRequest cancel_request =
+            hardware.has_cancel_request();
         if (clear_queue_until_empty) {
             // If we were executing a move when estop asserted, and
             // what's in the queue is the remaining enqueued moves from
@@ -206,14 +207,12 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (has_cancel_request != 0U) {
-            if (has_cancel_request ==
-                static_cast<uint8_t>(can::ids::ErrorSeverity::unrecoverable)) {
-                cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error_detected); // use cancel_request.code?
-            } else {
-                cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
-                                       can::ids::ErrorSeverity::warning);
-            }
+        } else if (cancel_request.code !=
+                   0U) {  // is this correct? Should be zero-initialized. Check,
+                          // confirm!
+            cancel_and_clear_moves(
+                static_cast<can::ids::ErrorCode>(cancel_request.code),
+                static_cast<can::ids::ErrorSeverity>(cancel_request.severity));
         } else {
             // Normal Move logic
             run_normal_interrupt();

include/motor-control/core/stepper_motor/tmc2130_driver.hpp

@@ -52,8 +52,8 @@ class TMC2130 {
     auto operator=(const TMC2130&& c) = delete;
     ~TMC2130() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0)
-        -> void {
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index,
+              uint8_t tags = 0) -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
         uint32_t token = spi::utils::build_token(converted_addr, tags);
         _spi_manager.read(token, command_data, _task_queue, _cs_intf,

include/motor-control/core/stepper_motor/tmc2160_driver.hpp

@@ -53,8 +53,8 @@ class TMC2160 {
     auto operator=(const TMC2160&& c) = delete;
     ~TMC2160() = default;
 
-    auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0)
-        -> void {
+    auto read(Registers addr, uint32_t command_data, uint32_t message_index,
+              uint8_t tags = 0) -> void {
         auto converted_addr = static_cast<uint8_t>(addr);
         uint32_t token = spi::utils::build_token(converted_addr, tags);
         _spi_manager.read(token, command_data, _task_queue, _cs_intf,

include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp

@@ -82,7 +82,8 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
@@ -91,7 +92,8 @@ class MotorDriverMessageHandler {
         uint32_t data = 0;
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
         uint8_t tag_byte = spi::utils::byte_from_tags(tags);
-        driver.read(tmc2160::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::GearWriteMotorCurrentRequest& m) {

include/motor-control/core/tasks/motion_controller_task.hpp

@@ -147,17 +147,19 @@ class MotionControllerMessageHandler {
     void handle(const can::messages::MotorDriverErrorEncountered& m) {
         if (!driver_error_handled()) {
             // check if gpio is low before making controller calls
-            controller.stop(can::ids::ErrorSeverity::unrecoverable);
+            controller.stop(can::ids::ErrorSeverity::unrecoverable,
+                            can::ids::ErrorCode::motor_driver_error_detected);
             if (!controller.is_timer_interrupt_running()) {
                 can_client.send_can_message(
                     can::ids::NodeId::host,
                     can::messages::ErrorMessage{
                         .message_index = m.message_index,
                         .severity = can::ids::ErrorSeverity::unrecoverable,
-                        .error_code = can::ids::ErrorCode::motor_driver_error_detected});
+                        .error_code =
+                            can::ids::ErrorCode::motor_driver_error_detected});
                 driver_client.send_motor_driver_queue(
-                    can::messages::ReadMotorDriverErrorStatus{.message_index =
-                                                                m.message_index});
+                    can::messages::ReadMotorDriverErrorStatus{
+                        .message_index = m.message_index});
             }
         }
     }
@@ -225,9 +227,8 @@ class MotionControllerTask {
 
     // also create top level query msg
     void run_diag0_interrupt() {
-        static_cast<void>(
-            queue.try_write_isr(can::messages::MotorDriverErrorEncountered{
-                .message_index = 0}));
+        static_cast<void>(queue.try_write_isr(
+            can::messages::MotorDriverErrorEncountered{.message_index = 0}));
     }
 
   private:

include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp

@@ -81,16 +81,18 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2130::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2130::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2130::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
     void handle(const can::messages::ReadMotorDriverErrorStatus& m) {
         LOG("Received read motor driver error register request");
-        uint32_t data = 0;        
+        uint32_t data = 0;
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
         uint8_t tag_byte = spi::utils::byte_from_tags(tags);
-        driver.read(tmc2130::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte);
+        driver.read(tmc2130::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {

include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp

@@ -94,7 +94,8 @@ class MotorDriverMessageHandler {
         LOG("Received read motor driver request: addr=%d", m.reg_address);
         uint32_t data = 0;
         if (tmc2160::registers::is_valid_address(m.reg_address)) {
-            driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index);
+            driver.read(tmc2160::registers::Registers(m.reg_address), data,
+                        m.message_index);
         }
     }
 
@@ -103,7 +104,8 @@ class MotorDriverMessageHandler {
         uint32_t data = 0;
         std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE};
         uint8_t tag_byte = spi::utils::byte_from_tags(tags);
-        driver.read(tmc2160::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte);
+        driver.read(tmc2160::registers::Registers::DRVSTATUS, data,
+                    m.message_index, tag_byte);
     }
 
     void handle(const can::messages::WriteMotorCurrentRequest& m) {

include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp

@@ -45,6 +45,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
           controller_loop{config.pid_kp,  config.pid_ki,
                           config.pid_kd,  1.F / config.encoder_interrupt_freq,
                           config.wl_high, config.wl_low},
+          cancel_request(),
           eeprom_config{eeprom_config},
           stopwatch_handle{stopwatch_handle} {}
     BrushedMotorHardware(const BrushedMotorHardware&) = delete;
@@ -78,14 +79,21 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void reset_control() final;
     void set_stay_enabled(bool state) final { stay_enabled = state; }
     auto get_stay_enabled() -> bool final { return stay_enabled; }
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
+    // need any std::optional usage?
+    auto has_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(can::ids::ErrorSeverity error_severity,
+                        can::ids::ErrorCode error_code) final {
+        CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
     }
     void clear_cancel_request() final {
-        cancel_request.store(0);
+        CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
         return eeprom_config;
@@ -106,7 +114,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     int32_t motor_encoder_overflow_count = 0;
     ot_utils::pid::PID controller_loop;
     std::atomic<ControlDirection> control_dir = ControlDirection::unset;
-    std::atomic<uint8_t> cancel_request = 0;
+    std::atomic<CancelRequest> cancel_request;
     const UsageEEpromConfig& eeprom_config;
     void* stopwatch_handle;
     BrushedMotorState motor_state = BrushedMotorState::UNHOMED;

include/motor-control/firmware/stepper_motor/motor_hardware.hpp

@@ -31,7 +31,8 @@ class MotorHardware : public StepperMotorHardwareIface {
         : pins(config),
           tim_handle(timer_handle),
           enc_handle(encoder_handle),
-          eeprom_config(eeprom_config) {}
+          eeprom_config(eeprom_config),
+          cancel_request() {}
     MotorHardware(const MotorHardware&) = delete;
     auto operator=(const MotorHardware&) -> MotorHardware& = delete;
     MotorHardware(MotorHardware&&) = delete;
@@ -57,14 +58,21 @@ class MotorHardware : public StepperMotorHardwareIface {
     void disable_encoder() final;
     void enable_encoder() final;
 
-    auto has_cancel_request() -> uint8_t final {
-        return cancel_request.exchange(0);
+    // need any std::optional usage?
+    auto has_cancel_request() -> CancelRequest final {
+        CancelRequest exchange_request;
+        return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(uint8_t error_severity) final {
-        cancel_request.store(error_severity);
+    void request_cancel(can::ids::ErrorSeverity error_severity,
+                        can::ids::ErrorCode error_code) final {
+        CancelRequest update_request{
+            .severity = static_cast<uint8_t>(error_severity),
+            .code = static_cast<uint8_t>(error_code)};
+        cancel_request.store(update_request);
     }
     void clear_cancel_request() final {
-        cancel_request.store(0);
+        CancelRequest clear_request;
+        cancel_request.store(clear_request);
     }
 
     auto get_usage_eeprom_config() -> const UsageEEpromConfig& final {
@@ -83,7 +91,7 @@ class MotorHardware : public StepperMotorHardwareIface {
     void* enc_handle;
     const UsageEEpromConfig& eeprom_config;
     std::atomic<int32_t> motor_encoder_overflow_count = 0;
-    std::atomic<uint8_t> cancel_request = 0;
+    std::atomic<CancelRequest> cancel_request;
     static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31;
 };