Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-23 00:59:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

integrate SPI arbiter with SPI encoder

Browse Source
This commit is contained in:
Samuel Sadok
2020-07-21 12:31:03 +02:00
parent 856455077d
commit b9b70b9c0a
9 changed files with 91 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/Board/v3/Inc/board.h
+3
View File
@@ -37,6 +37,9 @@ extern Motor m0;
extern Motor m1;
extern OnboardThermistorCurrentLimiter m0_fet_thermistor;
extern OnboardThermistorCurrentLimiter m1_fet_thermistor;
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
extern Stm32SpiArbiter& ext_spi_arbiter;
#endif
// Period in [s]
Firmware/Board/v3/board.cpp
+1
View File
@@ -5,6 +5,7 @@
#include <board.h>
Stm32SpiArbiter spi3_arbiter{&hspi3};
Stm32SpiArbiter& ext_spi_arbiter = spi3_arbiter;
Drv8301 m0_gate_driver{
&spi3_arbiter,
Firmware/Drivers/STM32/stm32_gpio.hpp
+1
View File
@@ -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_; }
Firmware/Drivers/STM32/stm32_spi_arbiter.cpp
+25 -37
View File
@@ -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
Firmware/Drivers/STM32/stm32_spi_arbiter.hpp
+17 -2
View File
@@ -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.
Firmware/MotorControl/encoder.cpp
+35 -33
View File
@@ -3,9 +3,10 @@
#include <Drivers/STM32/stm32_system.h>
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;
Firmware/MotorControl/encoder.hpp
+7 -2
View File
@@ -1,6 +1,9 @@
#ifndef __ENCODER_HPP
#define __ENCODER_HPP
#include <arm_math.h>
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
#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;
Firmware/MotorControl/main.cpp
+1 -1
View File
@@ -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]);
tools/odrive/tests/calibration_test.py
+1 -1
View File
@@ -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)