draft

Author: ahiuchingau

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

@@ -2,6 +2,7 @@
 
 #include "FreeRTOS.h"
 #include "firmware/motor_hardware.h"
+#include "firmware/motor_driver_policy.hpp"
 #include "flex-stacker/motor_driver_task.hpp"
 #include "ot_utils/freertos/freertos_timer.hpp"
 
@@ -28,7 +29,9 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
 
     spi_hardware_init();
 
+    auto policy = motor_driver_policy::MotorDriverPolicy();
     while (true) {
+        _top_task.run_once(policy);
     }
 }
 

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

@@ -0,0 +1,13 @@
+#include "firmware/motor_driver_policy.hpp"
+
+#include "firmware/motor_hardware.h"
+
+
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto MotorDriverPolicy::tmc2160_transmit_receive(MotorID motor_id, tmc2160::MessageT& data) -> RxTxReturn {
+    tmc2160::MessageT retBuf = {0};
+    if (spi_dma_transmit_receive(motor_id, data.data(), retBuf.data(), data.size())) {
+        return RxTxReturn(retBuf);
+    }
+    return RxTxReturn();
+}

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

@@ -5,8 +5,11 @@
 #include "stm32g4xx_hal_gpio.h"
 #include "stm32g4xx_hal_dma.h"
 
+#include "FreeRTOS.h"
+#include "task.h"
 #include <stdbool.h>
 
+
 /* SPI2 Pins */
 #define SPI2_SCK_Pin (GPIO_PIN_13)
 #define SPI2_CIPO_Pin (GPIO_PIN_14)
@@ -23,21 +26,29 @@
 #define nSPI2_NSS_L_Pin (GPIO_PIN_2)
 #define nSPI2_NSS_L_GPIO_Port (GPIOB)
 
+/** Maximum length of a SPI transaction is 5 bytes.*/
+#define MOTOR_MAX_SPI_LEN (5)
+
 
 /** Static Variables -------------------------------------------------------- */
 
 struct motor_spi_hardware {
     SPI_HandleTypeDef handle;
+    TaskHandle_t task_to_notify;
     bool initialized;
 };
 
+static void spi_interrupt_service(void);
+static void spi_set_nss(bool selected);
+
 
 DMA_HandleTypeDef hdma_spi2_rx;
 DMA_HandleTypeDef hdma_spi2_tx;
 
 
 static struct motor_spi_hardware _spi = {
     .handle = {0},
+    .task_to_notify = NULL,
     .initialized = false
 };
 
@@ -246,36 +257,60 @@ void SPI2_IRQHandler(void)
     HAL_SPI_IRQHandler(&_spi.handle);
 }
 
+static void spi_interrupt_service(void) {
+    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
+    if( _spi.task_to_notify == NULL ) {
+        return;
+    }
+    vTaskNotifyGiveFromISR( _spi.task_to_notify, &xHigherPriorityTaskWoken );
+    _spi.task_to_notify = NULL;
+    portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
+}
+
+
 /**
   * @brief TxRx Transfer completed callback.
   */
 void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
 {
-    // TODO: Implement this callback
+    spi_interrupt_service();
 }
 
 /**
   * @brief Spi Transfer Error callback.
  */
 void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
 {
-    // TODO: Implement this callback
+    spi_interrupt_service();
 }
 
-void spi_dma_transmit_receive(
+bool tmc2160_transmit_receive(
     MotorID motor_id, uint8_t *txData, uint8_t *rxData, uint16_t size
 ) {
+    const TickType_t max_block_time = pdMS_TO_TICKS(100);
+    HAL_StatusTypeDef ret;
+    uint32_t notification_val = 0;
+
+    if (!_spi.initialized || (_spi.task_to_notify != NULL) || (size > MOTOR_MAX_SPI_LEN)) {
+        return false;
+    }
     // enable one of the motor driver for SPI communication
     enable_spi_nss(motor_id);
-
+    _spi.task_to_notify = xTaskGetCurrentTaskHandle();
     if (HAL_SPI_TransmitReceive_DMA(&_spi.handle, txData, rxData, size) != HAL_OK) {
         // Transmission error
-        // TODO: implement error handling
+        _spi.task_to_notify = NULL;
+        return false;
     }
-
-    // Wait for the transmission to complete
-    while (HAL_SPI_GetState(&_spi.handle) != HAL_SPI_STATE_READY) {}
-
+    notification_val = ulTaskNotifyTake(pdTRUE, max_block_time);
     disable_spi_nss();
+    // If the task was preempted by the error handler rather than the
+    // TxRx complete callback, the remaining count should be greater
+    // than 0.
+    if((notification_val != 1) || (_spi.handle.RxXferCount > 0)) {
+        _spi.task_to_notify = NULL;
+        return true;
+    }
+    return true;
 }
 

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

@@ -0,0 +1,15 @@
+#pragma once
+
+#include <optional>
+
+#include "systemwide.h"
+#include "flex-stacker/tmc2160.hpp"
+
+namespace motor_driver_policy {
+class MotorDriverPolicy {
+  public:
+    using RxTxReturn = std::optional<tmc2160::MessageT>;
+    auto tmc2160_transmit_receive(MotorID motor_id, tmc2160::MessageT& data) -> RxTxReturn;
+};
+
+}  // namespace motor_driver_policy

stm32-modules/include/flex-stacker/firmware/motor_driver_policy.hpp

@@ -4,14 +4,15 @@
 extern "C" {
 #endif  // __cplusplus
 
+#include <stdbool.h>
 #include <stdint.h>
 
 #include "systemwide.h"
 
 void motor_hardware_init(void);
 
 void spi_hardware_init(void);
-void spi_dma_transmit_receive(MotorID motor_id, uint8_t *tx_data,
+bool spi_dma_transmit_receive(MotorID motor_id, uint8_t *tx_data,
                               uint8_t *rx_data, uint16_t len);
 
 #ifdef __cplusplus

stm32-modules/include/flex-stacker/firmware/motor_hardware.h

@@ -11,8 +11,6 @@ class MotorPolicy {
     auto enable_motor(MotorID motor_id) -> void;
     auto disable_motor(MotorID motor_id) -> void;
     auto set_motor_speed(MotorID motor_id, double speed) -> bool;
-
-    auto spi_transmit_receive(MotorID motor_id, uint8_t *tx_data,
-                              uint8_t *rx_data, uint16_t len) -> void;
 };
+
 }  // namespace motor_policy

stm32-modules/include/flex-stacker/firmware/motor_policy.hpp

@@ -5,26 +5,34 @@
  */
 #pragma once
 
+
+#include "systemwide.h"
 #include "core/ack_cache.hpp"
 #include "core/queue_aggregator.hpp"
 #include "core/version.hpp"
+#include "firmware/motor_policy.hpp"
 #include "flex-stacker/messages.hpp"
 #include "flex-stacker/tasks.hpp"
+#include "flex-stacker/tmc2160.hpp"
+#include "flex-stacker/tmc2160_interface.hpp"
 #include "flex-stacker/tmc2160_registers.hpp"
 #include "hal/message_queue.hpp"
 
 namespace motor_driver_task {
 
+
 using Message = messages::MotorDriverMessage;
 
+
 static constexpr tmc2160::TMC2160RegisterMap motor_z_config{
     .gconfig = {.diag0_error = 1, .diag1_stall = 1},
+    .glob_scale = {.global_scaler = 0x8B},
     .ihold_irun = {.hold_current = 1,
                    .run_current = 31,
                    .hold_current_delay = 1},
+    .tpwmthrs = {.threshold = 0x80000},
     .tcoolthrs = {.threshold = 0x81},
     .thigh = {.threshold = 0x81},
-    .tpwmthrs = {.threshold = 0x80000},
     .chopconf = {.toff = 0b111,
                  .hstrt = 0b100,
                  .hend = 0b11,
@@ -37,17 +45,17 @@ static constexpr tmc2160::TMC2160RegisterMap motor_z_config{
                 .pwm_autograd = 1,
                 .pwm_reg = 4,
                 .pwm_lim = 0xC},
-    .glob_scale = {.global_scaler = 0x8B},
 };
 
 static constexpr tmc2160::TMC2160RegisterMap motor_x_config{
     .gconfig = {.diag0_error = 1, .diag1_stall = 1},
+    .glob_scale = {.global_scaler = 0x8B},
     .ihold_irun = {.hold_current = 1,
                    .run_current = 31,
                    .hold_current_delay = 1},
+    .tpwmthrs = {.threshold = 0x80000},
     .tcoolthrs = {.threshold = 0x81},
     .thigh = {.threshold = 0x81},
-    .tpwmthrs = {.threshold = 0x80000},
     .chopconf = {.toff = 0b111,
                  .hstrt = 0b111,
                  .hend = 0b1001,
@@ -60,17 +68,17 @@ static constexpr tmc2160::TMC2160RegisterMap motor_x_config{
                 .pwm_autograd = 1,
                 .pwm_reg = 4,
                 .pwm_lim = 0xC},
-    .glob_scale = {.global_scaler = 0x8B},
 };
 
-static constexpr tmc2160::TMC2160RegisterMap motor_l_config{
+constexpr tmc2160::TMC2160RegisterMap motor_l_config{
     .gconfig = {.diag0_error = 1, .diag1_stall = 1},
+    .glob_scale = {.global_scaler = 0x8B},
     .ihold_irun = {.hold_current = 1,
                    .run_current = 31,
                    .hold_current_delay = 1},
+    .tpwmthrs = {.threshold = 0x80000},
     .tcoolthrs = {.threshold = 0x81},
     .thigh = {.threshold = 0x81},
-    .tpwmthrs = {.threshold = 0x80000},
     .chopconf = {.toff = 0b111,
                  .hstrt = 0b111,
                  .hend = 0b1001,
@@ -83,7 +91,6 @@ static constexpr tmc2160::TMC2160RegisterMap motor_l_config{
                 .pwm_autograd = 1,
                 .pwm_reg = 4,
                 .pwm_lim = 0xC},
-    .glob_scale = {.global_scaler = 0x8B},
 };
 
 template <template <class> class QueueImpl>
@@ -107,10 +114,24 @@ class MotorDriverTask {
         _task_registry = aggregator;
     }
 
-    auto run_once() -> void {
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto run_once(Policy& policy) -> void {
         if (!_task_registry) {
             return;
         }
+        if (!_initialized) {
+
+            if (!_tmc2160.initialize_config(motor_z_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_Z)) {
+                return;
+            }
+            if (!_tmc2160.initialize_config(motor_x_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_X)) {
+                return;
+            }
+            if (!_tmc2160.initialize_config(motor_l_config, tmc2160::TMC2160Interface(policy), MotorID::MOTOR_L)) {
+                return;
+            }
+            _initialized = true;
+        }
 
         auto message = Message(std::monostate());
 
@@ -126,6 +147,7 @@ class MotorDriverTask {
         static_cast<void>(message);
     }
 
+
     auto visit_message(const messages::SetMotorDriverRegister& message) -> void {
         static_cast<void>(message);
     }
@@ -142,9 +164,14 @@ class MotorDriverTask {
         static_cast<void>(message);
     }
 
+
+
     Queue& _message_queue;
     Aggregator* _task_registry;
     bool _initialized;
-};
 
+    tmc2160::TMC2160 _tmc2160{};
+    tmc2160::TMC2160Interface<Policy>* _tmc2160_interface;
+
+};
 };  // namespace motor_task