finished pipette subsystem implementation for testing

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

@@ -50,6 +50,7 @@ concept TaskClient = requires(Client client, const TaskMessage& m) {
 template <typename Client>
 concept GearTaskClient = requires(Client client, const GearTaskMessage& m) {
     {client.send_motor_driver_queue(m)};
+    {client.send_motor_driver_queue_isr(m)};
 };
 };  // namespace tasks
 };  // namespace tmc

include/pipettes/core/gear_motor_tasks.hpp

@@ -31,15 +31,17 @@ using CanWriterTask = can::message_writer_task::MessageWriterTask<
 using SPIWriterClient =
     spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>;
 
-void start_tasks(
+auto start_tasks(
     CanWriterTask& can_writer,
     interfaces::gear_motor::GearMotionControl& motion_controllers,
     SPIWriterClient& spi_writer,
     motor_configs::HighThroughputPipetteDriverHardware& gear_driver_configs,
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor);
+        tail_accessor) -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler>;
+
+void call_run_diag0_interrupt();
 
 /**
  * Access to all the gear motion tasks.

include/pipettes/core/interfaces.hpp

@@ -32,12 +32,8 @@ struct HighThroughputInterruptQueues {
     UpdatePositionQueue left_update_queue;
 };
 
-namespace linear_motor {
-
 extern "C" using diag0_handler = void(*)();
 
-}
-
 namespace gear_motor {
 
 struct UnavailableGearHardware {};

include/pipettes/core/linear_motor_tasks.hpp

@@ -40,7 +40,7 @@ auto start_tasks(
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> interfaces::linear_motor::diag0_handler;
+        tail_accessor) -> interfaces::diag0_handler;
 
 // 96/384 linear motor tasks
 auto start_tasks(
@@ -51,7 +51,7 @@ auto start_tasks(
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> interfaces::linear_motor::diag0_handler;
+        tail_accessor) -> interfaces::diag0_handler;
 
 void call_run_diag0_interrupt();
 

include/pipettes/core/tasks/messages.hpp

@@ -10,13 +10,19 @@ namespace task_messages {
 
 namespace motor_control_task_messages {
 
+struct RouteMotorDriverInterrupt {
+    uint32_t message_index;
+    uint8_t debounce_count;
+};
+
 using MotionControlTaskMessage = std::variant<
     std::monostate, can::messages::TipActionRequest,
     can::messages::GearDisableMotorRequest,
     can::messages::GearEnableMotorRequest,
     can::messages::GetMotionConstraintsRequest,
     can::messages::SetMotionConstraints, can::messages::StopRequest,
-    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest>;
+    can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest,
+    RouteMotorDriverInterrupt>;
 
 using MoveStatusReporterTaskMessage =
     std::variant<std::monostate, motor_messages::GearMotorAck,

include/pipettes/core/tasks/motion_controller_task.hpp

@@ -8,6 +8,7 @@
 #include "motor-control/core/linear_motion_system.hpp"
 #include "motor-control/core/stepper_motor/motion_controller.hpp"
 #include "motor-control/core/tasks/usage_storage_task.hpp"
+#include "motor-control/core/tasks/tmc_motor_driver_common.hpp"
 #include "pipettes/core/tasks/messages.hpp"
 
 namespace pipettes {
@@ -19,22 +20,38 @@ namespace motion_controller_task {
 using TaskMessage = pipettes::task_messages::motor_control_task_messages::
     MotionControlTaskMessage;
 
+/**
+ * Concept describing a class that can message this task.
+ * @tparam Client
+ */
+template <typename Client>
+concept TaskClient = requires(Client client, const TaskMessage& m) {
+    {client.send_motion_controller_queue(m)};
+};
+
 /**
  * The message queue message handler.
  */
 template <lms::MotorMechanicalConfig MEConfig,
           can::message_writer_task::TaskClient CanClient,
-          usage_storage_task::TaskClient UsageClient>
+          usage_storage_task::TaskClient UsageClient,
+          tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient>
 class MotionControllerMessageHandler {
   public:
     using MotorControllerType =
         pipette_motion_controller::PipetteMotionController<MEConfig>;
     MotionControllerMessageHandler(MotorControllerType& controller,
                                    CanClient& can_client,
-                                   UsageClient& usage_client)
+                                   UsageClient& usage_client,
+                                   DriverClient& driver_client,
+                                   MotionClient& motion_client,
+                                   std::atomic<bool>& diag0_debounced)
         : controller{controller},
           can_client{can_client},
-          usage_client{usage_client} {}
+          usage_client{usage_client},
+          driver_client{driver_client},
+          motion_client{motion_client},
+          diag0_debounced{diag0_debounced} {}
     MotionControllerMessageHandler(const MotionControllerMessageHandler& c) =
         delete;
     MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) =
@@ -125,9 +142,44 @@ class MotionControllerMessageHandler {
         controller.send_usage_data(m.message_index, usage_client);
     }
 
+    void handle(const pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt& m) {
+        if (m.debounce_count > 9) {
+            if (controller.read_tmc_diag0()) {
+                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});
+                    driver_client.send_motor_driver_queue(
+                        can::messages::ReadMotorDriverErrorStatusRequest{
+                            .message_index = m.message_index});
+                }
+            } else {
+                controller.clear_cancel_request();
+            }
+            diag0_debounced = false;
+        } else {
+            vTaskDelay(pdMS_TO_TICKS(100));
+            motion_client.send_motion_controller_queue(increment_message_debounce_count(m));
+        }
+    }
+
+    auto increment_message_debounce_count(const pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt& m) -> pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt {
+        return pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index, .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)};
+    }
+
     MotorControllerType& controller;
     CanClient& can_client;
     UsageClient& usage_client;
+    DriverClient& driver_client;
+    MotionClient& motion_client;
+    std::atomic<bool>& diag0_debounced;
 };
 
 /**
@@ -151,13 +203,15 @@ class MotionControllerTask {
      */
     template <lms::MotorMechanicalConfig MEConfig,
               can::message_writer_task::TaskClient CanClient,
-              usage_storage_task::TaskClient UsageClient>
+              usage_storage_task::TaskClient UsageClient,
+              tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient>
     [[noreturn]] void operator()(
         pipette_motion_controller::PipetteMotionController<MEConfig>*
             controller,
-        CanClient* can_client, UsageClient* usage_client) {
+        CanClient* can_client, UsageClient* usage_client,
+        DriverClient* driver_client, MotionClient* motion_client) {
         auto handler = MotionControllerMessageHandler{*controller, *can_client,
-                                                      *usage_client};
+                                                      *usage_client, *driver_client, *motion_client, diag0_debounced};
         TaskMessage message{};
         for (;;) {
             if (queue.try_read(&message, queue.max_delay)) {
@@ -168,17 +222,18 @@ class MotionControllerTask {
 
     [[nodiscard]] auto get_queue() const -> QueueType& { return queue; }
 
+    void run_diag0_interrupt() {
+        if (!diag0_debounced) {
+            static_cast<void>(queue.try_write_isr(
+                pipettes::task_messages::motor_control_task_messages::RouteMotorDriverInterrupt{
+                    .message_index = 0, .debounce_count = 0}));
+            diag0_debounced = true;
+        }
+    }
+
   private:
     QueueType& queue;
-};
-
-/**
- * Concept describing a class that can message this task.
- * @tparam Client
- */
-template <typename Client>
-concept TaskClient = requires(Client client, const TaskMessage& m) {
-    {client.send_motion_controller_queue(m)};
+    std::atomic<bool> diag0_debounced = false;
 };
 
 }  // namespace motion_controller_task

pipettes/core/gear_motor_tasks.cpp

@@ -37,15 +37,30 @@ static auto move_status_task_builder_right = freertos_task::TaskStarter<
 static auto left_usage_storage_task_builder =
     freertos_task::TaskStarter<256, usage_storage_task::UsageStorageTask>{};
 
-void gear_motor_tasks::start_tasks(
+
+void call_run_diag0_left_interrupt() {
+    if (gear_motor_tasks::get_left_gear_tasks().motion_controller) {
+        return gear_motor_tasks::get_left_gear_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+void call_run_diag0_right_interrupt() {
+    if (gear_motor_tasks::get_right_gear_tasks().motion_controller) {
+        return gear_motor_tasks::get_right_gear_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
+}
+
+auto gear_motor_tasks::start_tasks(
     gear_motor_tasks::CanWriterTask& can_writer,
     interfaces::gear_motor::GearMotionControl& motion_controllers,
     gear_motor_tasks::SPIWriterClient& spi_writer,
     motor_configs::HighThroughputPipetteDriverHardware& gear_driver_configs,
     can::ids::NodeId id,
     interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) {
+        tail_accessor) -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler> {
     left_queue_client.set_node_id(id);
     right_queue_client.set_node_id(id);
 
@@ -57,6 +72,7 @@ void gear_motor_tasks::start_tasks(
     // Left Gear Motor Tasks
     auto& motion_left = mc_task_builder_left.start(5, "motion controller",
                                                    motion_controllers.left,
+                                                   left_queues, left_queues,
                                                    left_queues, left_queues);
     auto& tmc2160_driver_left = tmc2160_driver_task_builder_left.start(
         5, "tmc2160 driver", gear_driver_configs.left_gear_motor, left_queues,
@@ -87,7 +103,7 @@ void gear_motor_tasks::start_tasks(
     // Right Gear Motor Tasks
     auto& motion_right = mc_task_builder_right.start(
         5, "motion controller", motion_controllers.right, right_queues,
-        right_queues);
+        right_queues, right_queues, right_queues);
     auto& tmc2160_driver_right = tmc2160_driver_task_builder_right.start(
         5, "tmc2160 driver", gear_driver_configs.right_gear_motor, right_queues,
         spi_writer);
@@ -116,6 +132,9 @@ void gear_motor_tasks::start_tasks(
 
     gmh_tsks.left.start_task();
     gmh_tsks.right.start_task();
+
+    return std::make_tuple(call_run_diag0_left_interrupt,
+                           call_run_diag0_right_interrupt);
 }
 
 gear_motor_tasks::QueueClient::QueueClient()

pipettes/core/linear_motor_tasks.cpp

@@ -39,7 +39,7 @@ auto linear_motor_tasks::start_tasks(
     tmc2130::configs::TMC2130DriverConfig& linear_driver_configs,
     can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> interfaces::linear_motor::diag0_handler {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2130_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 
@@ -93,7 +93,7 @@ auto linear_motor_tasks::start_tasks(
     tmc2160::configs::TMC2160DriverConfig& linear_driver_configs,
     can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk,
     eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>&
-        tail_accessor) -> interfaces::linear_motor::diag0_handler {
+        tail_accessor) -> interfaces::diag0_handler {
     tmc2160_queue_client.set_node_id(id);
     motion_queue_client.set_node_id(id);
 

pipettes/firmware/main.cpp

@@ -45,7 +45,9 @@ constexpr auto PIPETTE_TYPE = get_pipette_type();
 
 static auto iWatchdog = iwdg::IndependentWatchDog{};
 
-static interfaces::linear_motor::diag0_handler call_diag0_handler = nullptr;
+static interfaces::diag0_handler call_linear_diag0_handler = nullptr;
+static interfaces::diag0_handler call_left_gear_diag0_handler = nullptr;
+static interfaces::diag0_handler call_right_gear_diag0_handler = nullptr;
 
 static auto can_bus_1 = can::hal::bus::HalCanBus(
     can_get_device_handle(), utility_configs::led_gpio(PIPETTE_TYPE));
@@ -147,9 +149,19 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
                     sensors::tip_presence::TipStatusChangeDetected{}));
         }
     } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
-        if (call_diag0_handler != nullptr) {
-            if (*call_diag0_handler != nullptr) {
-                (*call_diag0_handler)();
+        if (call_linear_diag0_handler != nullptr) {
+            if (*call_linear_diag0_handler != nullptr) {
+                (*call_linear_diag0_handler)();
+            }
+        }
+        if (call_left_gear_diag0_handler != nullptr) {
+            if (*call_left_gear_diag0_handler != nullptr) {
+                (*call_left_gear_diag0_handler)();
+            }
+        }
+        if (call_right_gear_diag0_handler != nullptr) {
+            if (*call_right_gear_diag0_handler != nullptr) {
+                (*call_right_gear_diag0_handler)();
             }
         }
     }
@@ -196,11 +208,11 @@ auto initialize_motor_tasks(
     initialize_linear_timer(plunger_callback);
     initialize_gear_timer(gear_callback_wrapper);
     initialize_enc_timer(encoder_callback);
-    call_diag0_handler = linear_motor_tasks::start_tasks(
+    call_linear_diag0_handler = linear_motor_tasks::start_tasks(
         *central_tasks::get_tasks().can_writer, linear_motion_control,
         peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk,
         tail_accessor);
-    gear_motor_tasks::start_tasks(
+    std::tie(call_left_gear_diag0_handler, call_right_gear_diag0_handler) = gear_motor_tasks::start_tasks(
         *central_tasks::get_tasks().can_writer, gear_motion,
         peripheral_tasks::get_spi_client(), conf, id, gmh_tsks, tail_accessor);
 }
@@ -231,7 +243,7 @@ auto initialize_motor_tasks(
 
     initialize_linear_timer(plunger_callback);
     initialize_enc_timer(encoder_callback);
-    call_diag0_handler = linear_motor_tasks::start_tasks(
+    call_linear_diag0_handler = linear_motor_tasks::start_tasks(
         *central_tasks::get_tasks().can_writer, linear_motion_control,
         peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk,
         tail_accessor);