From b9b70b9c0a4333ae21c57e1d1c5cc85c486e13e7 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 17 Jun 2020 12:33:09 +0200 Subject: [PATCH] integrate SPI arbiter with SPI encoder --- Firmware/Board/v3/Inc/board.h | 3 + Firmware/Board/v3/board.cpp | 1 + Firmware/Drivers/STM32/stm32_gpio.hpp | 1 + Firmware/Drivers/STM32/stm32_spi_arbiter.cpp | 62 +++++++----------- Firmware/Drivers/STM32/stm32_spi_arbiter.hpp | 19 +++++- Firmware/MotorControl/encoder.cpp | 68 ++++++++++---------- Firmware/MotorControl/encoder.hpp | 9 ++- Firmware/MotorControl/main.cpp | 2 +- tools/odrive/tests/calibration_test.py | 2 +- 9 files changed, 91 insertions(+), 76 deletions(-) diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 2dfb9289..d845204b 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -37,6 +37,9 @@ extern Motor m0; extern Motor m1; extern OnboardThermistorCurrentLimiter m0_fet_thermistor; extern OnboardThermistorCurrentLimiter m1_fet_thermistor; + +#include +extern Stm32SpiArbiter& ext_spi_arbiter; #endif // Period in [s] diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index eca62cc1..a63edf3a 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -5,6 +5,7 @@ #include Stm32SpiArbiter spi3_arbiter{&hspi3}; +Stm32SpiArbiter& ext_spi_arbiter = spi3_arbiter; Drv8301 m0_gate_driver{ &spi3_arbiter, diff --git a/Firmware/Drivers/STM32/stm32_gpio.hpp b/Firmware/Drivers/STM32/stm32_gpio.hpp index e5636fc6..b90af00b 100644 --- a/Firmware/Drivers/STM32/stm32_gpio.hpp +++ b/Firmware/Drivers/STM32/stm32_gpio.hpp @@ -5,6 +5,7 @@ class Stm32Gpio { public: + Stm32Gpio() : port_(nullptr), pin_(0) {} Stm32Gpio(GPIO_TypeDef* port, uint16_t pin) : port_(port), pin_(pin) {} operator bool() const { return port_; } diff --git a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp index 093e1453..8ee1a3a2 100644 --- a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp +++ b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp @@ -46,21 +46,7 @@ bool Stm32SpiArbiter::start() { return status == HAL_OK; } -bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const uint8_t* tx_buf, uint8_t* rx_buf, size_t length, uint32_t timeout_ms) { - bool done = false; - - SpiTask task = { - .config = config, - .ncs_gpio = ncs_gpio, - .tx_buf = tx_buf, - .rx_buf = rx_buf, - .length = length, - .on_complete = [](void* ctx) { *(bool*)ctx = true; }, - .cb_ctx = &done, - .next = nullptr - }; - - +void Stm32SpiArbiter::transfer_async(SpiTask* task) { // Append new task to task list. // We could try to do this lock free but we could also use our time for useful things. SpiTask** ptr = &task_list_; @@ -68,39 +54,41 @@ bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const uint32_t prim = cpu_enter_critical(); while (*ptr) ptr = &(*ptr)->next; - *ptr = &task; + *ptr = task; cpu_exit_critical(prim); } // If the list was empty before, kick off the SPI arbiter now if (ptr == &task_list_) { if (!start()) { - return false; + if (task->on_complete) { + (*task->on_complete)(task->cb_ctx, false); + } } } +} - while (!done) { +bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const uint8_t* tx_buf, uint8_t* rx_buf, size_t length, uint32_t timeout_ms) { + uint8_t result = 0xff; + + SpiTask task = { + .config = config, + .ncs_gpio = ncs_gpio, + .tx_buf = tx_buf, + .rx_buf = rx_buf, + .length = length, + .on_complete = [](void* ctx, bool success) { *(uint8_t*)ctx = success ? 1 : 0; }, + .cb_ctx = &result, + .next = nullptr + }; + + transfer_async(&task); + + while (result == 0xff) { osDelay(1); // TODO: honor timeout } - return true; - - -/* HAL_StatusTypeDef status = HAL_ERROR; -// delay_us(1); - task.ncs_gpio.write(false); -// delay_us(1); - if (task.tx_buf && task.rx_buf) { - status = HAL_SPI_TransmitReceive(hspi_, (uint8_t*)task.tx_buf, task.rx_buf, task.length, 1000); - } else if (task.tx_buf) { - status = HAL_SPI_Transmit(hspi_, (uint8_t*)task.tx_buf, task.length, 1000); - } else if (task.rx_buf) { - status = HAL_SPI_Receive(hspi_, task.rx_buf, task.length, 1000); - } -// delay_us(1); - task.ncs_gpio.write(true); -// delay_us(1); - return status == HAL_OK;*/ + return result; } void Stm32SpiArbiter::on_complete() { @@ -111,7 +99,7 @@ void Stm32SpiArbiter::on_complete() { // Wrap up transfer task_list_->ncs_gpio.write(true); if (task_list_->on_complete) { - (*task_list_->on_complete)(task_list_->cb_ctx); + (*task_list_->on_complete)(task_list_->cb_ctx, true); } // Start next task if any diff --git a/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp b/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp index 59c1772a..58f9d540 100644 --- a/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp +++ b/Firmware/Drivers/STM32/stm32_spi_arbiter.hpp @@ -13,13 +13,27 @@ public: const uint8_t* tx_buf; uint8_t* rx_buf; size_t length; - void (*on_complete)(void*); + void (*on_complete)(void*, bool); void* cb_ctx; struct SpiTask* next; }; Stm32SpiArbiter(SPI_HandleTypeDef* hspi): hspi_(hspi) {} + /** + * @brief Enqueues a non-blocking transfer. + * + * Once the transfer completes, fails or is aborted, the callback is invoked. + * + * This function is thread-safe with respect to all other public functions + * of this class. + * + * @param task: Contains all configuration data for this transfer. + * The struct pointed to by this argument must remain valid and + * unmodified until the completion callback is invoked. + */ + void transfer_async(SpiTask* task); + /** * @brief Executes a blocking transfer. * @@ -28,7 +42,8 @@ public: * * Returns true on successful transfer or false otherwise. * - * This function is thread-safe with respect to itself. + * This function is thread-safe with respect to all other public functions + * of this class. * * @param config: The SPI configuration to apply for this transfer. * @param ncs_gpio: The active low GPIO to actuate during this transfer. diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 62bb0b13..4a3ba555 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -3,9 +3,10 @@ #include -Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, +Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, Stm32SpiArbiter* spi_arbiter, Config_t& config, const Motor::Config_t& motor_config) : hw_config_(hw_config), + spi_arbiter_(spi_arbiter), config_(config) { update_pll_gains(); @@ -27,9 +28,24 @@ void Encoder::setup() { set_idx_subscribe(); mode_ = config_.mode; + + spi_task_.config = { + .Mode = SPI_MODE_MASTER, + .Direction = SPI_DIRECTION_2LINES, + .DataSize = SPI_DATASIZE_16BIT, + .CLKPolarity = mode_ == MODE_SPI_ABS_AEAT ? SPI_POLARITY_HIGH : SPI_POLARITY_LOW, + .CLKPhase = SPI_PHASE_2EDGE, + .NSS = SPI_NSS_SOFT, + .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32, + .FirstBit = SPI_FIRSTBIT_MSB, + .TIMode = SPI_TIMODE_DISABLE, + .CRCCalculation = SPI_CRCCALCULATION_DISABLE, + .CRCPolynomial = 10, + }; + if(mode_ & MODE_FLAG_ABS){ abs_spi_cs_pin_init(); - abs_spi_init(); + if (axis_->controller_.config_.anticogging.pre_calibrated) { axis_->controller_.anticogging_valid_ = true; } @@ -323,39 +339,27 @@ void Encoder::sample_now() { } } -bool Encoder::abs_spi_init(){ - if ((mode_ & MODE_FLAG_ABS) == 0x0) - return false; - - SPI_HandleTypeDef * spi = hw_config_.spi; - spi->Init.Mode = SPI_MODE_MASTER; - spi->Init.Direction = SPI_DIRECTION_2LINES; - spi->Init.DataSize = SPI_DATASIZE_16BIT; - spi->Init.CLKPolarity = SPI_POLARITY_LOW; - spi->Init.CLKPhase = SPI_PHASE_2EDGE; - spi->Init.NSS = SPI_NSS_SOFT; - spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32; - spi->Init.FirstBit = SPI_FIRSTBIT_MSB; - spi->Init.TIMode = SPI_TIMODE_DISABLE; - spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; - spi->Init.CRCPolynomial = 10; - if (mode_ == MODE_SPI_ABS_AEAT) { - spi->Init.CLKPolarity = SPI_POLARITY_HIGH; - } - HAL_SPI_DeInit(spi); - HAL_SPI_Init(spi); - return true; -} - bool Encoder::abs_spi_start_transaction(){ if (mode_ & MODE_FLAG_ABS){ axis_->motor_.log_timing(TIMING_LOG_SPI_START); - if(hw_config_.spi->State != HAL_SPI_STATE_READY){ - set_error(ERROR_ABS_SPI_NOT_READY); + + if (spi_busy_) { return false; } - HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET); - HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1); + + spi_task_.ncs_gpio = {abs_spi_cs_port_, abs_spi_cs_pin_}; + spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_; + spi_task_.rx_buf = (uint8_t*)abs_spi_dma_rx_; + spi_task_.length = 1; + spi_task_.on_complete = [](void* ctx, bool success) { + ((Encoder*)ctx)->spi_busy_ = false; + if (success) + ((Encoder*)ctx)->abs_spi_cb(); + }; + spi_task_.cb_ctx = this; + spi_task_.next = nullptr; + + spi_arbiter_->transfer_async(&spi_task_); } return true; } @@ -375,9 +379,7 @@ uint8_t cui_parity(uint16_t v) { return ~v & 3; } -void Encoder::abs_spi_cb(){ - HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET); - +void Encoder::abs_spi_cb() { axis_->motor_.log_timing(TIMING_LOG_SPI_END); uint16_t pos; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index e3c99c81..c7bb5509 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -1,6 +1,9 @@ #ifndef __ENCODER_HPP #define __ENCODER_HPP +#include + +#include #include "utils.hpp" class Encoder : public ODriveIntf::EncoderIntf { @@ -40,7 +43,7 @@ public: void set_bandwidth(float value) { bandwidth = value; parent->update_pll_gains(); } }; - Encoder(const EncoderHardwareConfig_t& hw_config, + Encoder(const EncoderHardwareConfig_t& hw_config, Stm32SpiArbiter* spi_arbiter, Config_t& config, const Motor::Config_t& motor_config); void setup(); @@ -63,6 +66,7 @@ public: bool update(); const EncoderHardwareConfig_t& hw_config_; + Stm32SpiArbiter* spi_arbiter_; Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor @@ -91,7 +95,6 @@ public: float sincos_sample_s_ = 0.0f; float sincos_sample_c_ = 0.0f; - bool abs_spi_init(); bool abs_spi_start_transaction(); void abs_spi_cb(); void abs_spi_cs_pin_init(); @@ -103,6 +106,8 @@ public: uint16_t abs_spi_cs_pin_; uint32_t abs_spi_cr1; uint32_t abs_spi_cr2; + bool spi_busy_ = false; + Stm32SpiArbiter::SpiTask spi_task_; constexpr float getCoggingRatio(){ return config_.cpr / 3600.0f; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index dac1be28..c5459a8e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -186,7 +186,7 @@ extern "C" int construct_objects(){ // Construct all objects. odCAN = new ODriveCAN(can_config, &hcan1); for (size_t i = 0; i < AXIS_COUNT; ++i) { - Encoder *encoder = new Encoder(hw_configs[i].encoder_config, + Encoder *encoder = new Encoder(hw_configs[i].encoder_config, &ext_spi_arbiter, encoder_configs[i], (i ? m1 : m0).config_); SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]); Controller *controller = new Controller(controller_configs[i]); diff --git a/tools/odrive/tests/calibration_test.py b/tools/odrive/tests/calibration_test.py index 4b9b6788..ff2585bf 100644 --- a/tools/odrive/tests/calibration_test.py +++ b/tools/odrive/tests/calibration_test.py @@ -161,7 +161,7 @@ class TestEncoderOffsetCalibration(): # run test request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) - time.sleep(9) # actual calibration takes 8 seconds + time.sleep(9.1) # actual calibration takes 9.0 seconds test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_no_error(axis_ctx)