feat(motion-control): handle motor driver errors

gantry/core/tasks_proto.cpp

@@ -55,18 +55,20 @@ static auto eeprom_data_rev_update_builder =
 /**
  * Start gantry tasks.
  */
-void gantry::tasks::start_tasks(
+auto gantry::tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2130::configs::TMC2130DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler {
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, ::queues, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues);
     auto& tmc2130_driver = motor_driver_task_builder.start(
         5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -116,6 +118,15 @@ void gantry::tasks::start_tasks(
     ::queues.usage_storage_queue = &usage_storage_task.get_queue();
 
     mh_tsk.start_task();
+
+    return gantry::tasks::call_run_diag0_interrupt;
+}
+
+void gantry::tasks::call_run_diag0_interrupt() {
+    if (gantry::tasks::get_tasks().motion_controller) {
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
 }
 
 gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)

gantry/core/tasks_rev1.cpp

@@ -54,18 +54,20 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues};
 /**
  * Start gantry ::tasks.
  */
-void gantry::tasks::start_tasks(
+auto gantry::tasks::start_tasks(
     can::bus::CanBus& can_bus,
     motion_controller::MotionController<lms::BeltConfig>& motion_controller,
     spi::hardware::SpiDeviceBase& spi_device,
     tmc2160::configs::TMC2160DriverConfig& driver_configs,
     motor_hardware_task::MotorHardwareTask& mh_tsk,
     i2c::hardware::I2CBase& i2c2,
-    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
+    eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
+    -> interfaces::diag0_handler {
     auto& can_writer = can_task::start_writer(can_bus);
     can_task::start_reader(can_bus);
-    auto& motion = mc_task_builder.start(5, "motion controller",
-                                         motion_controller, ::queues, ::queues);
+    auto& motion =
+        mc_task_builder.start(5, "motion controller", motion_controller,
+                              ::queues, ::queues, ::queues);
     auto& tmc2160_driver = motor_driver_task_builder.start(
         5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
     auto& move_group =
@@ -115,6 +117,15 @@ void gantry::tasks::start_tasks(
     ::queues.usage_storage_queue = &usage_storage_task.get_queue();
 
     mh_tsk.start_task();
+
+    return gantry::tasks::call_run_diag0_interrupt;
+}
+
+void gantry::tasks::call_run_diag0_interrupt() {
+    if (gantry::tasks::get_tasks().motion_controller) {
+        return gantry::tasks::get_tasks()
+            .motion_controller->run_diag0_interrupt();
+    }
 }
 
 gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)

gantry/firmware/interfaces_proto.cpp

@@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_5,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -156,7 +168,7 @@ static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{
     }};
 
 static tmc2130::configs::TMC2130DriverConfig gantry_y_driver_configs{
-    .registers = {.gconfig = {.en_pwm_mode = 1},
+    .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 0x2,
                                  .run_current = 0x18,
                                  .hold_current_delay = 0x7},
@@ -232,8 +244,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
-    update_position_queue);
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
+    motor_hardware_iface, stallcheck, update_position_queue);
 
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
@@ -269,13 +281,14 @@ static constexpr auto can_bit_timings =
     can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
                                  500 * can::bit_timings::KHZ, 800>{};
 
-void interfaces::initialize() {
+void interfaces::initialize(diag0_handler* call_diag0_handler) {
     // Initialize SPI
     if (initialize_spi(get_axis_type()) != HAL_OK) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, enc_overflow_callback);
+    initialize_timer(call_motor_handler, call_diag0_handler,
+                     enc_overflow_callback);
 
     // Start the can bus
     canbus.start(can_bit_timings);

gantry/firmware/interfaces_rev1.cpp

@@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
@@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y {
             .port = GPIOB,
             .pin = GPIO_PIN_7,
             .active_setting = GPIO_PIN_RESET},
-    .estop_in = {
+    .estop_in =
+        {
+            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
+            .port = GPIOA,
+            .pin = GPIO_PIN_10,
+            .active_setting = GPIO_PIN_RESET},
+    .diag0 = {
         // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast)
-        .port = GPIOA,
-        .pin = GPIO_PIN_10,
+        .port = GPIOC,
+        .pin = GPIO_PIN_5,
         .active_setting = GPIO_PIN_RESET}
 };
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -159,6 +171,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0x0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -172,7 +185,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{
     }};
 
 static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
-    .registers = {.gconfig = {.en_pwm_mode = 0},
+    .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1},
                   .ihold_irun = {.hold_current = 16,
                                  .run_current = 31,
                                  .hold_current_delay = 0x7},
@@ -198,6 +211,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{
                               .freewheel = 0,
                               .pwm_reg = 0x7,
                               .pwm_lim = 0xC},
+                  .drvconf = {.ot_select = 0x0},
                   .glob_scale = {.global_scaler = 0xA7}},
     .current_config =
         {
@@ -257,8 +271,8 @@ static stall_check::StallCheck stallcheck(
  * Handler of motor interrupts.
  */
 static motor_handler::MotorInterruptHandler motor_interrupt(
-    motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
-    update_position_queue);
+    motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
+    motor_hardware_iface, stallcheck, update_position_queue);
 
 static auto encoder_background_timer =
     motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
@@ -295,13 +309,14 @@ static constexpr auto can_bit_timings =
     can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
                                  500 * can::bit_timings::KHZ, 800>{};
 
-void interfaces::initialize() {
+void interfaces::initialize(diag0_handler* call_diag0_handler) {
     // Initialize SPI
     if (initialize_spi(get_axis_type()) != HAL_OK) {
         Error_Handler();
     }
 
-    initialize_timer(call_motor_handler, enc_overflow_callback);
+    initialize_timer(call_motor_handler, call_diag0_handler,
+                     enc_overflow_callback);
 
     // Start the can bus
     canbus.start(can_bit_timings);

gantry/firmware/main_proto.cpp

@@ -16,6 +16,8 @@
 #include "gantry/core/tasks_proto.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
+static interfaces::diag0_handler call_diag0_handler = nullptr;
+
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
 
@@ -44,11 +46,11 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    interfaces::initialize();
+    interfaces::initialize(&call_diag0_handler);
     i2c_setup(&i2c_handles);
     i2c_comms2.set_handle(i2c_handles.i2c2);
 
-    gantry::tasks::start_tasks(
+    call_diag0_handler = gantry::tasks::start_tasks(
         interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
         interfaces::get_spi(), interfaces::get_driver_config(),
         interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);

gantry/firmware/main_rev1.cpp

@@ -16,6 +16,8 @@
 #include "gantry/core/tasks_rev1.hpp"
 #include "i2c/firmware/i2c_comms.hpp"
 
+static interfaces::diag0_handler call_diag0_handler = nullptr;
+
 static auto i2c_comms2 = i2c::hardware::I2C();
 static auto i2c_handles = I2CHandlerStruct{};
 
@@ -44,11 +46,11 @@ auto main() -> int {
 
     app_update_clear_flags();
 
-    interfaces::initialize();
+    interfaces::initialize(&call_diag0_handler);
     i2c_setup(&i2c_handles);
     i2c_comms2.set_handle(i2c_handles.i2c2);
 
-    gantry::tasks::start_tasks(
+    call_diag0_handler = gantry::tasks::start_tasks(
         interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
         interfaces::get_spi(), interfaces::get_driver_config(),
         interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);

gantry/firmware/motor_hardware.c

@@ -23,6 +23,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         PC8   ------> Motor Step Pin
         Enable
         PA9   ------> Motor Enable Pin
+        DIAG0
+        PC5   ------> Motor Diag0 Pin
         */
         GPIO_InitStruct.Pin = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;
         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -54,6 +56,12 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
         GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
         HAL_GPIO_Init(GPIOA,  // NOLINT(cppcoreguidelines-pro-type-cstyle-cast)
                       &GPIO_InitStruct);
+
+        // Diag0
+        GPIO_InitStruct.Pin = GPIO_PIN_5;
+        GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
+        GPIO_InitStruct.Pull = GPIO_PULLUP;
+        HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
     }
 }
 SPI_HandleTypeDef hspi2 = {
@@ -87,7 +95,7 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) {
         HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 |
                                    GPIO_PIN_15 | GPIO_PIN_1);
         HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9);
-        HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8);
+        HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8 | GPIO_PIN_5);
     }
 }
 
@@ -119,6 +127,7 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) {
 
 static motor_interrupt_callback timer_callback = NULL;
 static encoder_overflow_callback enc_overflow_callback = NULL;
+static diag0_interrupt_callback* diag0_callback = NULL;
 
 
 /**
@@ -140,6 +149,9 @@ void MX_GPIO_Init(void) {
     GPIO_InitStruct.Pull = GPIO_NOPULL;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
 }
 
 // motor timer: 200kHz from
@@ -236,6 +248,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
     }
 }
 
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    if (GPIO_Pin == GPIO_PIN_5) {
+        if (diag0_callback != NULL) {
+            if (*diag0_callback != NULL) {
+                (*diag0_callback)();
+            }
+        }
+    }
+}
+
 void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
     if (htim == &htim2) {
         /* Peripheral clock enable */
@@ -255,9 +277,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) {
     }
 }
 
-void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) {
+void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) {
     timer_callback = callback;
     enc_overflow_callback = enc_callback;
+    diag0_callback = diag0_int_callback;
     MX_GPIO_Init();
     MX_TIM7_Init();
     Encoder_GPIO_Init();