feat(flex-stacker): add the motor stallguard functionality and gcodes. (

#466)

stm32-modules/flex-stacker/firmware/main.cpp

@@ -1,11 +1,19 @@
 #include "FreeRTOS.h"
 #include "firmware/firmware_tasks.hpp"
 #include "firmware/freertos_tasks.hpp"
+#include "firmware/motor_hardware.h"
 #include "firmware/system_stm32g4xx.h"
+#include "flex-stacker/messages.hpp"
 #include "ot_utils/freertos/freertos_task.hpp"
 #include "systemwide.h"
 #include "task.h"
 
+#pragma GCC diagnostic push
+// NOLINTNEXTLINE(clang-diagnostic-unknown-warning-option)
+#pragma GCC diagnostic ignored "-Wvolatile"
+#include "stm32g4xx_hal.h"
+#pragma GCC diagnostic pop
+
 using EntryPoint = std::function<void(tasks::FirmwareTasks::QueueAggregator *)>;
 
 // NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
@@ -43,6 +51,17 @@ static auto ui_task =
     ot_utils::freertos_task::FreeRTOSTask<tasks::UI_STACK_SIZE, EntryPoint>(
         ui_task_entry);
 
+extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+    switch (GPIO_Pin) {
+        case MOTOR_DIAG0_PIN:
+            static_cast<void>(aggregator.send_from_isr(
+                messages::GPIOInterruptMessage{.pin = GPIO_Pin}));
+            break;
+        default:
+            break;
+    }
+}
+
 auto main() -> int {
     HardwareInit();
 

stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c

@@ -1,78 +1,16 @@
-
 #include "stm32g4xx_hal.h"
 #include "stm32g4xx_it.h"
+#include "motor_hardware.h"
 #include "systemwide.h"
 #include "FreeRTOS.h"
 
 #include <stdbool.h>
+#include <stdint.h>
 
 #ifdef __cplusplus
 extern "C" {
 #endif // __cplusplus
 
-/******************* Motor Z *******************/
-/** Motor hardware **/
-#define Z_STEP_PIN (GPIO_PIN_2)
-#define Z_STEP_PORT (GPIOC)
-#define Z_DIR_PIN (GPIO_PIN_1)
-#define Z_DIR_PORT (GPIOC)
-#define Z_EN_PIN (GPIO_PIN_3)
-#define Z_EN_PORT (GPIOA)
-#define Z_N_BRAKE_PIN (GPIO_PIN_7)
-#define Z_N_BRAKE_PORT (GPIOB)
-
-/** Limit switches **/
-/* Note: Photointerrupters limit switches */
-#define Z_MINUS_LIMIT_PIN (GPIO_PIN_3)
-#define Z_MINUS_LIMIT_PORT (GPIOC)
-#define Z_PLUS_LIMIT_PIN (GPIO_PIN_0)
-#define Z_PLUS_LIMIT_PORT (GPIOA)
-
-
-/******************* Motor X *******************/
-
-/** Motor hardware **/
-#define X_STEP_PIN (GPIO_PIN_7)
-#define X_STEP_PORT (GPIOA)
-#define X_DIR_PIN (GPIO_PIN_6)
-#define X_DIR_PORT (GPIOA)
-#define X_EN_PIN (GPIO_PIN_4)
-#define X_EN_PORT (GPIOA)
-#define X_N_BRAKE_PIN (GPIO_PIN_9)
-#define X_N_BRAKE_PORT (GPIOB)
-
-
-/** Limit switches **/
-/* Note: Photointerrupters limit switches */
-#define X_MINUS_LIMIT_PIN (GPIO_PIN_1)
-#define X_MINUS_LIMIT_PORT (GPIOA)
-#define X_PLUS_LIMIT_PIN (GPIO_PIN_2)
-#define X_PLUS_LIMIT_PORT (GPIOA)
-
-
-/******************* Motor L *******************/
-
-/** Motor hardware **/
-#define L_STEP_PIN (GPIO_PIN_1)
-#define L_STEP_PORT (GPIOB)
-#define L_DIR_PIN (GPIO_PIN_0)
-#define L_DIR_PORT (GPIOB)
-#define L_EN_PIN (GPIO_PIN_5)
-#define L_EN_PORT (GPIOC)
-
-
-/** Limit switches **/
-/* Note: Mechanical limit switches */
-#define L_N_HELD_PIN (GPIO_PIN_5)
-#define L_N_HELD_PORT (GPIOB)
-#define L_N_RELEASED_PIN (GPIO_PIN_11)
-#define L_N_RELEASED_PORT (GPIOC)
-
-/**************** COMMON ********************/
-#define ESTOP_PIN GPIO_PIN_6
-#define ESTOP_PORT GPIOB
-
-
 // Frequency of the motor interrupt callbacks is 300kHz, providing some extra
 // overhead over the velocities used by this application.
 #define MOTOR_INTERRUPT_FREQ (100000)
@@ -101,6 +39,7 @@ typedef struct stepper_hardware_struct {
     PinConfig limit_switch_minus;
     PinConfig limit_switch_plus;
     PinConfig ebrake;
+    PinConfig diag0;
 } stepper_hardware_t;
 
 typedef struct motor_hardware_struct {
@@ -120,6 +59,7 @@ static motor_hardware_t _motor_hardware = {
         .step = {X_STEP_PORT, X_STEP_PIN, GPIO_PIN_SET},
         .limit_switch_minus = {X_MINUS_LIMIT_PORT, X_MINUS_LIMIT_PIN, GPIO_PIN_SET},
         .limit_switch_plus = {X_PLUS_LIMIT_PORT, X_PLUS_LIMIT_PIN, GPIO_PIN_SET},
+        .diag0 = {MOTOR_DIAG0_PORT, MOTOR_DIAG0_PIN, GPIO_PIN_SET},
         .ebrake = {0},
     },
     .motor_z = {
@@ -129,6 +69,7 @@ static motor_hardware_t _motor_hardware = {
         .step = {Z_STEP_PORT, Z_STEP_PIN, GPIO_PIN_SET},
         .limit_switch_minus = {Z_MINUS_LIMIT_PORT, Z_MINUS_LIMIT_PIN, GPIO_PIN_SET},
         .limit_switch_plus = {Z_PLUS_LIMIT_PORT, Z_PLUS_LIMIT_PIN, GPIO_PIN_SET},
+        .diag0 = {MOTOR_DIAG0_PORT, MOTOR_DIAG0_PIN, GPIO_PIN_SET},
         .ebrake = {Z_N_BRAKE_PORT, Z_N_BRAKE_PIN, GPIO_PIN_RESET},
     },
     .motor_l = {
@@ -138,11 +79,11 @@ static motor_hardware_t _motor_hardware = {
         .step = {L_STEP_PORT, L_STEP_PIN, GPIO_PIN_SET},
         .limit_switch_minus = {L_N_HELD_PORT, L_N_HELD_PIN, GPIO_PIN_RESET},
         .limit_switch_plus = {L_N_RELEASED_PORT, L_N_RELEASED_PIN, GPIO_PIN_RESET},
+        .diag0 = {MOTOR_DIAG0_PORT, MOTOR_DIAG0_PIN, GPIO_PIN_SET},
         .ebrake = {0},
     }
 };
 
-
 void motor_hardware_gpio_init(void){
 
     GPIO_InitTypeDef init = {0};
@@ -168,9 +109,12 @@ void motor_hardware_gpio_init(void){
     init.Pin = Z_N_BRAKE_PIN;
     HAL_GPIO_Init(Z_N_BRAKE_PORT, &init);
 
-    init.Pin  = Z_STEP_PIN;
+    init.Pin = Z_STEP_PIN;
     HAL_GPIO_Init(Z_STEP_PORT, &init);
 
+    init.Pin = MOTOR_DIAG0_PIN;
+    HAL_GPIO_Init(MOTOR_DIAG0_PORT, &init);
+
     // X MOTOR
     init.Pin = X_EN_PIN;
     HAL_GPIO_Init(X_EN_PORT, &init);
@@ -193,6 +137,8 @@ void motor_hardware_gpio_init(void){
 
     /*Configure GPIO pins : INPUTS */
     init.Mode = GPIO_MODE_INPUT;
+    init.Pull = GPIO_NOPULL;
+    init.Speed = GPIO_SPEED_FREQ_LOW;
 
     // Z MOTOR
     init.Pin = Z_MINUS_LIMIT_PIN;
@@ -217,6 +163,15 @@ void motor_hardware_gpio_init(void){
 
     init.Pin = ESTOP_PIN;
     HAL_GPIO_Init(ESTOP_PORT, &init);
+
+    /*Configure GPIO pins : INPUTs IRQ */
+    init.Mode = GPIO_MODE_IT_FALLING;
+    init.Pull = GPIO_PULLUP;
+    init.Speed = GPIO_SPEED_FREQ_LOW;
+
+    init.Pin = MOTOR_DIAG0_PIN;
+    HAL_GPIO_Init(MOTOR_DIAG0_PORT, &init);
+    HAL_NVIC_SetPriority(EXTI15_10_IRQn, 6, 0);
 }
 
 // X motor timer
@@ -330,6 +285,7 @@ void motor_hardware_init(void){
         tim17_init(&_motor_hardware.motor_x.timer);
         tim20_init(&_motor_hardware.motor_z.timer);
         tim3_init(&_motor_hardware.motor_l.timer);
+
     }
     _motor_hardware.initialized = true;
 }
@@ -344,23 +300,15 @@ void hw_enable_ebrake(MotorID motor_id, bool enable) {
 
 stepper_hardware_t get_motor(MotorID motor_id) {
     switch (motor_id) {
-        case MOTOR_Z:
-            return _motor_hardware.motor_z;
-        case MOTOR_X:
-            return _motor_hardware.motor_x;
-        case MOTOR_L:
-            return _motor_hardware.motor_l;
-        default:
-            return (stepper_hardware_t){0};
+        case MOTOR_Z: return _motor_hardware.motor_z;
+        case MOTOR_X: return _motor_hardware.motor_x;
+        case MOTOR_L: return _motor_hardware.motor_l;
+        default: return (stepper_hardware_t){0};
     }
 }
 
 static uint8_t invert_gpio_value(uint8_t setting) {
-    if (setting == GPIO_PIN_SET) {
-        return GPIO_PIN_RESET;
-    } else {
-        return GPIO_PIN_SET;
-    }
+    return setting == GPIO_PIN_SET ? GPIO_PIN_RESET : GPIO_PIN_SET;
 }
 
 void set_pin(PinConfig config) {
@@ -417,6 +365,12 @@ bool hw_read_limit_switch(MotorID motor_id, bool direction) {
            motor.limit_switch_minus.active_setting;
 }
 
+void hw_set_diag0_irq(bool enable) {
+    enable ?
+        HAL_NVIC_EnableIRQ(EXTI15_10_IRQn) :
+        HAL_NVIC_DisableIRQ(EXTI15_10_IRQn);
+}
+
 void TIM3_IRQHandler(void)
 {
     HAL_TIM_IRQHandler(&_motor_hardware.motor_l.timer);

stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp

@@ -30,4 +30,9 @@ auto MotorPolicy::set_direction(MotorID motor_id, bool direction) -> void {
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto MotorPolicy::check_limit_switch(MotorID motor_id, bool direction) -> bool {
     return hw_read_limit_switch(motor_id, direction);
-}
+}
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorPolicy::set_diag0_irq(bool enable) -> void {
+    hw_set_diag0_irq(enable);
+}

stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c

@@ -121,3 +121,11 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
 void initialize_callbacks(motor_interrupt_callback callback_glue) {
     interrupt_callback = callback_glue;
 }
+
+// MOTOR_DIAG0_PIN interrupt
+void EXTI15_10_IRQHandler(void) {
+    if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_12)) {
+        HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12);
+    }
+}
+

stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h

@@ -34,6 +34,7 @@ void UsageFault_Handler(void);
 void DebugMon_Handler(void);
 // void SysTick_Handler(void);
 void RCC_IRQHandler(void);
+void EXTI15_10_IRQHandler(void);
 // void DMA1_Channel1_IRQHandler(void);
 // void DMA1_Channel2_IRQHandler(void);
 

stm32-modules/flex-stacker/src/errors.cpp

@@ -18,9 +18,11 @@ const char* const SYSTEM_EEPROM_ERROR =
 const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n";
 const char* const TMC2160_WRITE_ERROR = "ERR902:TMC2160 driver write error\n";
 const char* const TMC2160_INVALID_ADDRESS = "ERR903:TMC2160 invalid address\n";
+const char* const TMC2160_INVALID_VALUE = "ERR904:TMC2160 invalid value\n";
 
 const char* const MOTOR_ENABLE_FAILED = "ERR401:motor enable error\n";
-const char* const MOTOR_DISABLE_FAILED = "ERR401:motor disable error\n";
+const char* const MOTOR_DISABLE_FAILED = "ERR402:motor disable error\n";
+const char* const MOTOR_STALL_DETECTED = "ERR403:motor stall error\n";
 
 const char* const UNKNOWN_ERROR = "ERR-1:unknown error code\n";
 
@@ -43,8 +45,10 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(TMC2160_READ_ERROR);
         HANDLE_CASE(TMC2160_WRITE_ERROR);
         HANDLE_CASE(TMC2160_INVALID_ADDRESS);
+        HANDLE_CASE(TMC2160_INVALID_VALUE);
         HANDLE_CASE(MOTOR_ENABLE_FAILED);
         HANDLE_CASE(MOTOR_DISABLE_FAILED);
+        HANDLE_CASE(MOTOR_STALL_DETECTED);
     }
     return UNKNOWN_ERROR;
 }

stm32-modules/include/common/core/queue_aggregator.hpp

@@ -211,6 +211,21 @@ class QueueAggregator {
         return SendHelper<TaskCount>::send(msg, address, timeout_ms, this);
     }
 
+    /**
+     * @brief Send a message from isr and automatically deduce the mailbox
+     * to forward it to. This will fail if the type of the message
+     * is not unique.
+     *
+     * @tparam Message The type of message to send
+     * @param msg The message to send
+     * @return true if the message could be sent, false otherwise
+     */
+    template <typename Message>
+    auto send_from_isr(const Message& msg) -> bool {
+        constexpr auto idx = get_message_idx<Message>();
+        return send_from_isr_to<idx>(msg);
+    }
+
   private:
     /**
      * @brief Wrapper class for holding a pointer to a queue with
@@ -245,6 +260,24 @@ class QueueAggregator {
         return std::get<Idx>(_handles)._handle->try_send(msg, timeout_ms);
     }
 
+    /**
+     * @brief Internal function for sending a message from isr
+     *
+     * @tparam Idx The index of the message to send to, as returned by
+     *             \ref get_queue_idx()
+     * @tparam Message The type of message to send
+     * @param msg The message to send
+     * @return true if the message could be sent, false otherwise
+     */
+    template <size_t Idx, typename Message>
+    auto send_from_isr_to(const Message& msg) -> bool {
+        static_assert(Idx < TaskCount, "Invalid task index");
+        if (std::get<Idx>(_handles)._handle == nullptr) {
+            return false;
+        }
+        return std::get<Idx>(_handles)._handle->try_send_from_isr(msg);
+    }
+
     /**
      * @brief Get the index of a queue, based on an individual message
      * definition. This deduction will ONLY work if the message is unique