PR review changes part 3

Opentrons · Nov 3, 2023 · d2fd44c · d2fd44c
1 parent 352f8c8
commit d2fd44c
Show file tree

Hide file tree

Showing 26 changed files with 91 additions and 41 deletions.
diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c
@@ -59,7 +59,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
 
         // Diag0
         GPIO_InitStruct.Pin = GPIO_PIN_5;
-        GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
         GPIO_InitStruct.Pull = GPIO_PULLUP;
         HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }

diff --git a/gantry/firmware/stm32g4xx_it.c b/gantry/firmware/stm32g4xx_it.c
@@ -147,7 +147,7 @@ void FDCAN1_IT0_IRQHandler(void) {
  */
 void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); }
 void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
-void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); } // source this from somewhere?
+void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); }
 
 
 extern void xPortSysTickHandler(void);

diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp
@@ -45,6 +45,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<

diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp
@@ -65,6 +65,7 @@ enum class MessageId {
     read_motor_driver_error_status = 0x37,
     read_motor_driver_error_register_response = 0x38,
     reset_motor_driver_error_handling = 0x39,
+    route_motor_driver_interrupt = 0x3a,
     set_brushed_motor_vref_request = 0x40,
     set_brushed_motor_pwm_request = 0x41,
     gripper_grip_request = 0x42,

diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp
@@ -18,7 +18,7 @@ class MotionHandler {
                      GetMotionConstraintsRequest, SetMotionConstraints,
                      ReadLimitSwitchRequest, MotorPositionRequest,
                      UpdateMotorPositionEstimationRequest, GetMotorUsageRequest,
-                     MotorDriverErrorEncountered,
+                     RouteMotorDriverInterrupt, MotorDriverErrorEncountered,
                      ResetMotorDriverErrorHandling>;
 
     MotionHandler(MotionTaskClient &motion_client)

diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp
@@ -198,6 +198,9 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
 using UpdateMotorPositionEstimationRequest =
     Empty<MessageId::update_motor_position_estimation_request>;
 
+using RouteMotorDriverInterrupt =
+    Empty<MessageId::route_motor_driver_interrupt>;
+
 using MotorDriverErrorEncountered =
     Empty<MessageId::motor_driver_error_encountered>;
 

diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp
@@ -38,6 +38,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<

diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp
@@ -34,6 +34,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 using SystemDispatchTarget = can::dispatch::DispatchParseTarget<

diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
@@ -109,7 +109,7 @@ class MotionController {
         // 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(error_severity, error_code);
+                hardware.set_cancel_request(error_severity, error_code);
             }
         }
     }

diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
@@ -184,7 +184,7 @@ class BrushedMotorInterruptHandler {
         } else if (in_estop) {
             // if we've received a stop request during this time we can clear
             // that flag since there isn't anything running
-            std::ignore = hardware.has_cancel_request();
+            std::ignore = hardware.get_cancel_request();
             // return out of error state once the estop is disabled
             if (!estop_triggered()) {
                 in_estop = false;
@@ -201,7 +201,9 @@ class BrushedMotorInterruptHandler {
         } else if (estop_triggered()) {
             in_estop = true;
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
-        } else if (hardware.has_cancel_request().code != 0U) {
+        } else if (static_cast<can::ids::ErrorCode>(
+                       hardware.get_cancel_request().code) !=
+                   can::ids::ErrorCode::ok) {
             if (!hardware.get_stay_enabled()) {
                 hardware.set_motor_state(BrushedMotorState::UNHOMED);
             }

diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp
@@ -96,6 +96,7 @@ class MotorHardwareIface {
     virtual void read_limit_switch() = 0;
     virtual void read_estop_in() = 0;
     virtual void read_sync_in() = 0;
+    virtual bool read_tmc_diag0() = 0;
     virtual auto get_encoder_pulses() -> int32_t = 0;
     virtual void reset_encoder_pulses() = 0;
     virtual void start_timer_interrupt() = 0;
@@ -104,9 +105,9 @@ class MotorHardwareIface {
     virtual void enable_encoder() = 0;
     virtual void disable_encoder() = 0;
 
-    virtual auto has_cancel_request() -> CancelRequest = 0;
-    virtual void request_cancel(can::ids::ErrorSeverity error_severity,
-                                can::ids::ErrorCode error_code) = 0;
+    virtual auto get_cancel_request() -> CancelRequest = 0;
+    virtual void set_cancel_request(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;
 

diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -113,7 +113,7 @@ class MotionController {
         can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) {
         queue.reset();
         if (hardware.is_timer_interrupt_running()) {
-            hardware.request_cancel(error_severity, error_code);
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
@@ -137,6 +137,8 @@ class MotionController {
 
     auto check_read_sync_line() -> bool { return hardware.check_sync_in(); }
 
+    auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); }
+
     void enable_motor() {
         // check motor driver error register
         hardware.start_timer_interrupt();
@@ -277,14 +279,16 @@ class PipetteMotionController {
         // 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(error_severity, error_code);
+            hardware.set_cancel_request(error_severity, error_code);
         }
     }
 
     auto read_limit_switch() -> bool { return hardware.check_limit_switch(); }
 
     auto check_read_sync_line() -> bool { return hardware.check_sync_in(); }
 
+    auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); }
+
     void enable_motor() {
         hardware.start_timer_interrupt();
         hardware.activate_motor();

diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -188,7 +188,7 @@ class MotorInterruptHandler {
     void run_interrupt() {
         // handle various error states
         motor_hardware::CancelRequest cancel_request =
-            hardware.has_cancel_request();
+            hardware.get_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
@@ -207,9 +207,10 @@ class MotorInterruptHandler {
         } else if (estop_triggered()) {
             cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
             in_estop = true;
-        } else if (cancel_request.code !=
-                   0U) {  // is this correct? Should be zero-initialized. Check,
-                          // confirm!
+        } else if (static_cast<can::ids::ErrorCode>(cancel_request.code) !=
+                   can::ids::ErrorCode::ok) {  // 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));

diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp
@@ -163,9 +163,6 @@ class TMC2160 {
             case Registers::DRVSTATUS:
                 update_driver_status(0);
                 break;
-            // case Registers::DRV_CONF:
-            //     update_drv_conf(0);
-            //     break;
             default:
                 break;
         }
@@ -318,9 +315,9 @@ class TMC2160 {
      * be read, so this function gets it from the actual device.
      */
     auto update_glob_scaler(uint32_t data) -> void {
-        auto ret = read_register<GConfig>(data);
+        auto ret = read_register<GConfig>(data);  // should be GlobalScalar
         if (ret.has_value()) {
-            _registers.gconfig = ret.value();
+            _registers.gconfig = ret.value();  // should be glob_scale
         }
     }
 

diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp
@@ -15,6 +15,7 @@ using MotionControlTaskMessage = std::variant<
     can::messages::HomeRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;
 

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -144,9 +144,19 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(const can::messages::RouteMotorDriverInterrupt& m) {
+        static_cast<void>(m);
+        if (controller.read_tmc_diag0()) {  // debounce needed?
+            handle_message(
+                can::messages::MotorDriverErrorEncountered{.message_index = 0});
+        } else {
+            handle_message(can::messages::ResetMotorDriverErrorHandling{
+                .message_index = 0});
+        }
+    }
+
     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,
                             can::ids::ErrorCode::motor_driver_error_detected);
             if (!controller.is_timer_interrupt_running()) {
@@ -225,10 +235,9 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
-    // also create top level query msg
     void run_diag0_interrupt() {
         static_cast<void>(queue.try_write_isr(
-            can::messages::MotorDriverErrorEncountered{.message_index = 0}));
+            can::messages::RouteMotorDriverInterrupt{.message_index = 0}));
     }
 
   private:

diff --git a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp
@@ -58,12 +58,25 @@ class MotorDriverMessageHandler {
             auto data = driver.handle_spi_read(
                 tmc2130::registers::Registers(static_cast<uint8_t>(m.id.token)),
                 m.rxBuffer);
-            can::messages::ReadMotorDriverRegisterResponse response_msg{
-                .message_index = m.id.message_index,
-                .reg_address = static_cast<uint8_t>(m.id.token),
-                .data = data,
-            };
-            can_client.send_can_message(can::ids::NodeId::host, response_msg);
+            if (spi::utils::tag_in_token(
+                    m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) {
+                can::messages::ReadMotorDriverErrorRegisterResponse
+                    response_msg{
+                        .message_index = m.id.message_index,
+                        .reg_address = static_cast<uint8_t>(m.id.token),
+                        .data = data,
+                    };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            } else {
+                can::messages::ReadMotorDriverRegisterResponse response_msg{
+                    .message_index = m.id.message_index,
+                    .reg_address = static_cast<uint8_t>(m.id.token),
+                    .data = data,
+                };
+                can_client.send_can_message(can::ids::NodeId::host,
+                                            response_msg);
+            }
         }
     }
 

diff --git a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp
@@ -62,6 +62,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
+    bool read_tmc_diag0() final;
     void grip() final;
     void ungrip() final;
     void stop_pwm() final;
@@ -79,12 +80,12 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface {
     void set_stay_enabled(bool state) final { stay_enabled = state; }
     auto get_stay_enabled() -> bool final { return stay_enabled; }
     // need any std::optional usage?
-    auto has_cancel_request() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+    void set_cancel_request(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)};

diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp
@@ -51,19 +51,20 @@ class MotorHardware : public StepperMotorHardwareIface {
     void read_limit_switch() final;
     void read_estop_in() final;
     void read_sync_in() final;
+    bool read_tmc_diag0() final;
     void set_LED(bool status) final;
     auto get_encoder_pulses() -> int32_t final;
     void reset_encoder_pulses() final;
     void disable_encoder() final;
     void enable_encoder() final;
 
     // need any std::optional usage?
-    auto has_cancel_request() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest exchange_request = {};
         return cancel_request.exchange(exchange_request);
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
-                        can::ids::ErrorCode error_code) final {
+    void set_cancel_request(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)};

diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp
@@ -42,6 +42,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     void stop_pwm() final { move_dir = PWM_DIRECTION::unset; }
     auto check_sync_in() -> bool final { return sync_val; }
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return diag0_val; }
 
     auto get_encoder_pulses() -> int32_t final {
         return (motor_encoder_overflow_count << 16) + enc_val;
@@ -71,13 +72,13 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     PWM_DIRECTION get_direction() { return move_dir; }
     void set_stay_enabled(bool state) { stay_enabled = state; }
     auto get_stay_enabled() -> bool { return stay_enabled; }
-    auto has_cancel_request() -> CancelRequest final {
+    auto get_cancel_request() -> CancelRequest final {
         CancelRequest old_request = cancel_request;
         CancelRequest exchange_request{};
         cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
                         can::ids::ErrorCode error_code) final {
         CancelRequest update_request{
             .severity = static_cast<uint8_t>(error_severity),
@@ -108,6 +109,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface {
     int32_t motor_encoder_overflow_count = 0;
     bool ls_val = false;
     bool sync_val = false;
+    bool diag0_val = false;
     bool estop_in_val = false;
     bool is_gripping = false;
     bool motor_enabled = false;

diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp
@@ -30,6 +30,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     void read_limit_switch() final {}
     void read_estop_in() final {}
     void read_sync_in() final {}
+    bool read_tmc_diag0() final { return mock_diag0_value; }
     void set_LED(bool) final {}
     void set_mock_lim_sw(bool value) { mock_lim_sw_value = value; }
     void set_mock_estop_in(bool value) { mock_estop_in_value = value; }
@@ -39,13 +40,13 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     void reset_encoder_pulses() final { test_pulses = 0; }
     int32_t get_encoder_pulses() final { return test_pulses; }
     void sim_set_encoder_pulses(int32_t pulses) { test_pulses = pulses; }
-    auto has_cancel_request() -> motor_hardware::CancelRequest final {
+    auto get_cancel_request() -> motor_hardware::CancelRequest final {
         motor_hardware::CancelRequest old_request = cancel_request;
         motor_hardware::CancelRequest exchange_request{};
         cancel_request = exchange_request;
         return old_request;
     }
-    void request_cancel(can::ids::ErrorSeverity error_severity,
+    void set_cancel_request(can::ids::ErrorSeverity error_severity,
                         can::ids::ErrorCode error_code) final {
         motor_hardware::CancelRequest update_request{
             .severity = static_cast<uint8_t>(error_severity),
@@ -72,6 +73,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface {
     bool mock_lim_sw_value = false;
     bool mock_estop_in_value = false;
     bool mock_sync_value = false;
+    bool mock_diag0_value = false;
     bool mock_sr_value = false;
     bool mock_dir_value = false;
     uint8_t finished_move_id = 0x0;

diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp
@@ -66,6 +66,7 @@ using MotionControllerDispatchTarget = can::dispatch::DispatchParseTarget<
     can::messages::MotorPositionRequest,
     can::messages::UpdateMotorPositionEstimationRequest,
     can::messages::GetMotorUsageRequest,
+    can::messages::RouteMotorDriverInterrupt,
     can::messages::MotorDriverErrorEncountered,
     can::messages::ResetMotorDriverErrorHandling>;