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

move ext int subscribe logic to Stm32Gpio class

Browse Source
This commit is contained in:
Samuel Sadok
2020-07-21 12:31:03 +02:00
parent ab7957f07f
commit 6377df4b1e
20 changed files with 449 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+1
View File
@@ -47,6 +47,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint
* Use DMA for DRV8301 setup
* Make NVM configuration code more dynamic so that the layout doesn't have to be known at compile time
* Refactor GPIO code. Note that if two components use the same interrupt pin (e.g. step input for axis0 and axis1) then previously the one that was configured later would override the other one. Now this is no longer the case (the old component remains the owner of the pin).
# Releases
## [0.4.12] - 2020-05-06
Firmware/Board/v3/Inc/board.h
+10 -43
View File
@@ -35,13 +35,11 @@ using TGateDriver = Drv8301;
using TOpAmp = Drv8301;
#include <MotorControl/motor.hpp>
#include <MotorControl/encoder.hpp>
extern Motor m0;
extern Motor m1;
extern OnboardThermistorCurrentLimiter m0_fet_thermistor;
extern OnboardThermistorCurrentLimiter m1_fet_thermistor;
extern Motor* motors[AXIS_COUNT];
extern OnboardThermistorCurrentLimiter* fet_thermistors[AXIS_COUNT];
extern Motor motors[AXIS_COUNT];
extern OnboardThermistorCurrentLimiter fet_thermistors[AXIS_COUNT];
extern Encoder encoders[AXIS_COUNT];
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
extern Stm32SpiArbiter& ext_spi_arbiter;
@@ -60,18 +58,6 @@ typedef struct {
osPriority thread_priority;
} AxisHardwareConfig_t;
typedef struct {
TIM_HandleTypeDef* timer;
GPIO_TypeDef* index_port;
uint16_t index_pin;
GPIO_TypeDef* hallA_port;
uint16_t hallA_pin;
GPIO_TypeDef* hallB_port;
uint16_t hallB_pin;
GPIO_TypeDef* hallC_port;
uint16_t hallC_pin;
SPI_HandleTypeDef* spi;
} EncoderHardwareConfig_t;
typedef struct {
SPI_HandleTypeDef* spi;
GPIO_TypeDef* enable_port;
@@ -83,7 +69,6 @@ typedef struct {
} GateDriverHardwareConfig_t;
typedef struct {
AxisHardwareConfig_t axis_config;
EncoderHardwareConfig_t encoder_config;
} BoardHardwareConfig_t;
extern const BoardHardwareConfig_t hw_configs[2];
@@ -98,18 +83,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
.dir_gpio_pin = 2,
.thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
},
.encoder_config = {
.timer = &htim3,
.index_port = M0_ENC_Z_GPIO_Port,
.index_pin = M0_ENC_Z_Pin,
.hallA_port = M0_ENC_A_GPIO_Port,
.hallA_pin = M0_ENC_A_Pin,
.hallB_port = M0_ENC_B_GPIO_Port,
.hallB_pin = M0_ENC_B_Pin,
.hallC_port = M0_ENC_Z_GPIO_Port,
.hallC_pin = M0_ENC_Z_Pin,
.spi = &hspi3,
},
},{
//M1
.axis_config = {
@@ -122,18 +95,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
#endif
.thread_priority = osPriorityHigh,
},
.encoder_config = {
.timer = &htim4,
.index_port = M1_ENC_Z_GPIO_Port,
.index_pin = M1_ENC_Z_Pin,
.hallA_port = M1_ENC_A_GPIO_Port,
.hallA_pin = M1_ENC_A_Pin,
.hallB_port = M1_ENC_B_GPIO_Port,
.hallB_pin = M1_ENC_B_Pin,
.hallC_port = M1_ENC_Z_GPIO_Port,
.hallC_pin = M1_ENC_Z_Pin,
.spi = &hspi3,
},
} };
#endif
@@ -144,6 +105,12 @@ const BoardHardwareConfig_t hw_configs[2] = { {
#define I2C_A2_PORT GPIO_5_GPIO_Port
#define I2C_A2_PIN GPIO_5_Pin
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4
#define GPIO_COUNT 5
#else
#define GPIO_COUNT 8
#endif
// This board has no board-specific user configurations
static inline bool board_pop_config() { return true; }
static inline bool board_push_config() { return true; }
Firmware/Board/v3/Inc/gpio.h
-13
View File
@@ -70,19 +70,6 @@ void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
void SetGPIO12toUART();
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down, void (*callback)(void*), void* ctx);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4
#define GPIO_COUNT 5
#else
#define GPIO_COUNT 8
#endif
/* USER CODE END Prototypes */
#ifdef __cplusplus
Firmware/Board/v3/Src/gpio.c
-143
View File
@@ -136,149 +136,6 @@ void MX_GPIO_Init(void)
/* USER CODE BEGIN 2 */
#endif // End GPIO Include
// @brief Returns the IRQ number associated with a certain pin.
// Note that all GPIOs with the same pin number map to the same IRQn,
// no matter which port they belong to.
IRQn_Type get_irq_number(uint16_t pin) {
uint16_t pin_number = 0;
pin >>= 1;
while (pin) {
pin >>= 1;
pin_number++;
}
switch (pin_number) {
case 0: return EXTI0_IRQn;
case 1: return EXTI1_IRQn;
case 2: return EXTI2_IRQn;
case 3: return EXTI3_IRQn;
case 4: return EXTI4_IRQn;
case 5:
case 6:
case 7:
case 8:
case 9: return EXTI9_5_IRQn;
case 10:
case 11:
case 12:
case 13:
case 14:
case 15: return EXTI15_10_IRQn;
default: return 0; // impossible
}
}
// @brief Puts the GPIO's 1 and 2 into UART mode.
// This will disable any interrupt subscribers of these GPIOs.
void SetGPIO12toUART() {
GPIO_InitTypeDef GPIO_InitStruct;
// make sure nothing is hogging the GPIO's
GPIO_unsubscribe(GPIO_1_GPIO_Port, GPIO_1_Pin);
GPIO_unsubscribe(GPIO_2_GPIO_Port, GPIO_2_Pin);
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
}
// Expected subscriptions: 2x step signal + 2x encoder index signal
#define MAX_SUBSCRIPTIONS 10
struct subscription_t {
GPIO_TypeDef* GPIO_port;
uint16_t GPIO_pin;
void (*callback)(void*);
void* ctx;
} subscriptions[MAX_SUBSCRIPTIONS] = { 0 };
size_t n_subscriptions = 0;
// Sets up the specified GPIO to trigger the specified callback
// on a rising edge of the GPIO.
// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down, void (*callback)(void*), void* ctx) {
// Register handler (or reuse existing registration)
// TODO: make thread safe
struct subscription_t* subscription = NULL;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin)
subscription = &subscriptions[i];
}
if (!subscription) {
if (n_subscriptions >= MAX_SUBSCRIPTIONS)
return false;
subscription = &subscriptions[n_subscriptions++];
}
*subscription = (struct subscription_t){
.GPIO_port = GPIO_port,
.GPIO_pin = GPIO_pin,
.callback = callback,
.ctx = ctx
};
// Set up GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = pull_up_down;
HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct);
// Clear any previous triggers
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_pin);
// Enable interrupt
HAL_NVIC_SetPriority(get_irq_number(GPIO_pin), 0, 0);
HAL_NVIC_EnableIRQ(get_irq_number(GPIO_pin));
return true;
}
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
bool is_pin_in_use = false;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin) {
subscriptions[i].callback = NULL;
subscriptions[i].ctx = NULL;
} else if (subscriptions[i].GPIO_pin == GPIO_pin) {
is_pin_in_use = true;
}
}
if (!is_pin_in_use)
HAL_NVIC_DisableIRQ(get_irq_number(GPIO_pin));
}
// @brief Configures the specified GPIO as an analog input.
// This disables any subscriptions that were active for this pin.
void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_unsubscribe(GPIO_port, GPIO_pin);
GPIO_InitStruct.Pin = GPIO_pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct);
}
//Dispatch processing of external interrupts based on source
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) {
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_pin == GPIO_pin) // TODO: check for port
if (subscriptions[i].callback)
subscriptions[i].callback(subscriptions[i].ctx);
}
}
/* USER CODE END 2 */
Firmware/Board/v3/Src/stm32f4xx_it.c
-77
View File
@@ -467,24 +467,6 @@ void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_t callback
}
}
/**
* @brief This function handles TIM1 update interrupt and TIM10 global interrupt.
*/
void TIM1_UP_TIM10_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
tim_update_cb(&htim1);
}
/**
* @brief This function handles TIM8 update interrupt and TIM13 global interrupt.
*/
void TIM8_UP_TIM13_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
tim_update_cb(&htim8);
}
/**
* @brief This function handles I2C1 event interrupt.
@@ -502,64 +484,5 @@ void I2C1_ER_IRQHandler(void)
HAL_I2C_ER_IRQHandler(&hi2c1);
}
/**
* @brief This function handles EXTI line0 interrupt.
*/
void EXTI0_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0);
}
/**
* @brief This function handles EXTI line2 interrupt.
*/
void EXTI2_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2);
}
/**
* @brief This function handles EXTI line3 interrupt.
*/
void EXTI3_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3);
}
/**
* @brief This function handles EXTI line4 interrupt.
*/
void EXTI4_IRQHandler(void)
{
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
}
/**
* @brief This function handles EXTI lines 5-9 interrupt.
*/
void EXTI9_5_IRQHandler(void)
{
// The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler()
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_8);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_9);
}
/**
* @brief This function handles EXTI lines 10-15 interrupt.
*/
void EXTI15_10_IRQHandler(void)
{
// The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler()
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_14);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
}
/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
Firmware/Board/v3/board.cpp
+61 -25
View File
@@ -25,42 +25,64 @@ const float fet_thermistor_poly_coeffs[] =
{363.93910201f, -462.15369634f, 307.55129571f, -27.72569531f};
const size_t fet_thermistor_num_coeffs = sizeof(fet_thermistor_poly_coeffs)/sizeof(fet_thermistor_poly_coeffs[1]);
OnboardThermistorCurrentLimiter m0_fet_thermistor{
15, // adc_channel
&fet_thermistor_poly_coeffs[0], // coefficients
fet_thermistor_num_coeffs // num_coeffs
};
OnboardThermistorCurrentLimiter m1_fet_thermistor{
OnboardThermistorCurrentLimiter fet_thermistors[AXIS_COUNT] = {
{
15, // adc_channel
&fet_thermistor_poly_coeffs[0], // coefficients
fet_thermistor_num_coeffs // num_coeffs
}, {
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
4, // adc_channel
4, // adc_channel
#else
1, // adc_channel
1, // adc_channel
#endif
&fet_thermistor_poly_coeffs[0], // coefficients
fet_thermistor_num_coeffs // num_coeffs
&fet_thermistor_poly_coeffs[0], // coefficients
fet_thermistor_num_coeffs // num_coeffs
}
};
Motor m0{
&htim1, // timer
TIM_1_8_PERIOD_CLOCKS, // control_deadline
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m0_gate_driver, // gate_driver
m0_gate_driver // opamp
Motor motors[AXIS_COUNT] = {
{
&htim1, // timer
TIM_1_8_PERIOD_CLOCKS, // control_deadline
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m0_gate_driver, // gate_driver
m0_gate_driver // opamp
},
{
&htim8, // timer
(3 * TIM_1_8_PERIOD_CLOCKS) / 2, // control_deadline
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m1_gate_driver, // gate_driver
m1_gate_driver // opamp
}
};
Motor m1{
&htim8, // timer
(3 * TIM_1_8_PERIOD_CLOCKS) / 2, // control_deadline
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m1_gate_driver, // gate_driver
m1_gate_driver // opamp
Encoder encoders[AXIS_COUNT] = {
{
&htim3, // timer
{M0_ENC_Z_GPIO_Port, M0_ENC_Z_Pin}, // index_gpio
{M0_ENC_A_GPIO_Port, M0_ENC_A_Pin}, // hallA_gpio
{M0_ENC_B_GPIO_Port, M0_ENC_B_Pin}, // hallB_gpio
{M0_ENC_Z_GPIO_Port, M0_ENC_Z_Pin}, // hallC_gpio
&spi3_arbiter // spi_arbiter
},
{
&htim4, // timer
{M1_ENC_Z_GPIO_Port, M1_ENC_Z_Pin}, // index_gpio
{M1_ENC_A_GPIO_Port, M1_ENC_A_Pin}, // hallA_gpio
{M1_ENC_B_GPIO_Port, M1_ENC_B_Pin}, // hallB_gpio
{M1_ENC_Z_GPIO_Port, M1_ENC_Z_Pin}, // hallC_gpio
&spi3_arbiter // spi_arbiter
}
};
Motor* motors[AXIS_COUNT] = {&m0, &m1};
OnboardThermistorCurrentLimiter* fet_thermistors[AXIS_COUNT] = {&m0_fet_thermistor, &m1_fet_thermistor};
extern "C" {
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
HAL_SPI_TxRxCpltCallback(hspi);
}
@@ -74,3 +96,17 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
spi3_arbiter.on_complete();
}
}
void TIM1_UP_TIM10_IRQHandler(void) {
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
motors[0].tim_update_cb();
}
void TIM8_UP_TIM13_IRQHandler(void) {
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
motors[1].tim_update_cb();
}
}
Firmware/Drivers/STM32/stm32_gpio.cpp
+170
View File
@@ -0,0 +1,170 @@
#include "stm32_gpio.hpp"
#define N_EXTI 16
struct subscription_t {
GPIO_TypeDef* port = nullptr;
void (*callback)(void*) = nullptr;
void* ctx = nullptr;
} subscriptions[N_EXTI];
/**
* @brief Returns the IRQ number associated with a certain pin.
* Note that all GPIOs with the same pin number map to the same IRQn,
* no matter which port they belong to.
*/
IRQn_Type get_irq_number(uint16_t pin_number) {
switch (pin_number) {
case 0: return EXTI0_IRQn;
case 1: return EXTI1_IRQn;
case 2: return EXTI2_IRQn;
case 3: return EXTI3_IRQn;
case 4: return EXTI4_IRQn;
case 5:
case 6:
case 7:
case 8:
case 9: return EXTI9_5_IRQn;
case 10:
case 11:
case 12:
case 13:
case 14:
case 15: return EXTI15_10_IRQn;
default: return (IRQn_Type)0; // impossible
}
}
bool Stm32Gpio::subscribe(bool rising_edge, bool falling_edge, void (*callback)(void*), void* ctx) {
uint32_t pin_number = get_pin_number();
if (pin_number >= N_EXTI) {
return false; // invalid pin number
}
struct subscription_t& subscription = subscriptions[pin_number];
void (*no_port)(void*) = nullptr;
if (!__atomic_compare_exchange_n(&subscription.port, &no_port, port_, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)) {
return false; // already in use
}
subscription.ctx = ctx;
subscription.callback = callback;
// The following code is mostly taken from HAL_GPIO_Init
__HAL_RCC_SYSCFG_CLK_ENABLE();
uint32_t temp = SYSCFG->EXTICR[pin_number >> 2U];
temp &= ~(0x0FU << (4U * (pin_number & 0x03U)));
temp |= ((uint32_t)(GPIO_GET_INDEX(port_)) << (4U * (pin_number & 0x03U)));
SYSCFG->EXTICR[pin_number >> 2U] = temp;
if (rising_edge) {
EXTI->RTSR |= (uint32_t)pin_mask_;
} else {
EXTI->RTSR &= ~((uint32_t)pin_mask_);
}
if (falling_edge) {
EXTI->FTSR |= (uint32_t)pin_mask_;
} else {
EXTI->FTSR &= ~((uint32_t)pin_mask_);
}
EXTI->EMR &= ~((uint32_t)pin_mask_);
EXTI->IMR |= (uint32_t)pin_mask_;
// Clear any previous triggers
__HAL_GPIO_EXTI_CLEAR_IT(pin_mask_);
// Enable interrupt
// TODO: use configurable priority
HAL_NVIC_SetPriority(get_irq_number(pin_number), 0, 0);
HAL_NVIC_EnableIRQ(get_irq_number(pin_number));
return true;
}
void Stm32Gpio::unsubscribe() {
uint32_t pin_number = get_pin_number();
if (pin_number >= N_EXTI) {
return; // invalid pin number
}
struct subscription_t& subscription = subscriptions[pin_number];
HAL_NVIC_DisableIRQ(get_irq_number(pin_number));
EXTI->IMR |= (uint32_t)pin_mask_;
// At this point no more interrupts will be triggered for this GPIO
subscription.callback = nullptr;
subscription.ctx = nullptr;
subscription.port = nullptr; // after this line, the subscription can be reused (possibly by another thread)
}
void maybe_handle(uint16_t exti_number) {
if(__HAL_GPIO_EXTI_GET_IT(1 << exti_number) == RESET) {
return; // This interrupt source did not trigger the interrupt line
}
__HAL_GPIO_EXTI_CLEAR_IT(1 << exti_number);
if (exti_number >= N_EXTI) {
return;
}
subscription_t& subscription = subscriptions[exti_number];
if (subscription.callback) {
(*subscription.callback)(subscription.ctx);
}
}
extern "C" {
/** @brief Entrypoint for the EXTI line 0 interrupt. */
void EXTI0_IRQHandler(void) {
maybe_handle(0);
}
/** @brief Entrypoint for the EXTI line 1 interrupt. */
void EXTI1_IRQHandler(void) {
maybe_handle(1);
}
/** @brief Entrypoint for the EXTI line 2 interrupt. */
void EXTI2_IRQHandler(void) {
maybe_handle(2);
}
/** @brief Entrypoint for the EXTI line 3 interrupt. */
void EXTI3_IRQHandler(void) {
maybe_handle(3);
}
/** @brief Entrypoint for the EXTI line 4 interrupt. */
void EXTI4_IRQHandler(void) {
maybe_handle(4);
}
/** @brief Entrypoint for the EXTI lines 5-9 interrupt. */
void EXTI9_5_IRQHandler(void) {
maybe_handle(5);
maybe_handle(6);
maybe_handle(7);
maybe_handle(8);
maybe_handle(9);
}
/** @brief This function handles EXTI lines 10-15 interrupt. */
void EXTI15_10_IRQHandler(void) {
maybe_handle(10);
maybe_handle(11);
maybe_handle(12);
maybe_handle(13);
maybe_handle(14);
maybe_handle(15);
}
}
Firmware/Drivers/STM32/stm32_gpio.hpp
+61 -7
View File
@@ -5,22 +5,76 @@
class Stm32Gpio {
public:
Stm32Gpio() : port_(nullptr), pin_(0) {}
Stm32Gpio(GPIO_TypeDef* port, uint16_t pin) : port_(port), pin_(pin) {}
Stm32Gpio() : port_(nullptr), pin_mask_(0) {}
Stm32Gpio(GPIO_TypeDef* port, uint16_t pin) : port_(port), pin_mask_(pin) {}
operator bool() const { return port_; }
operator bool() const { return port_ && pin_mask_; }
/**
* @brief Configures the GPIO with the specified parameters.
*
* This can be done regardless of the current state of the GPIO.
*
* TODO: make glitch-free if the configuration is the same as before
* TODO: avoid disabling interrupt if it is enabled
*/
bool config(uint32_t mode, uint32_t pull) {
HAL_GPIO_DeInit(port_, pin_mask_);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = pin_mask_;
GPIO_InitStruct.Mode = mode;
GPIO_InitStruct.Pull = pull;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(port_, &GPIO_InitStruct);
return true;
}
void write(bool state) {
HAL_GPIO_WritePin(port_, pin_, state ? GPIO_PIN_SET : GPIO_PIN_RESET);
HAL_GPIO_WritePin(port_, pin_mask_, state ? GPIO_PIN_SET : GPIO_PIN_RESET);
}
bool read() {
return HAL_GPIO_ReadPin(port_, pin_) != GPIO_PIN_RESET;
return HAL_GPIO_ReadPin(port_, pin_mask_) != GPIO_PIN_RESET;
}
/**
* @brief Subscribes to external interrupts on the specified GPIO.
*
* Before calling this function the gpio should most likely be configured as
* input (however this is not mandatory, the interrupt works in output mode
* too).
*
* Only one subscription is allowed per pin number. I.e. it is not possible
* to set up a subscription for both PA0 and PB0 at the same time.
*
* This function is thread-safe with respect to all other public functions
* of this class.
*
* Returns true if the subscription was set up successfully or false otherwise.
*/
bool subscribe(bool rising_edge, bool falling_edge, void (*callback)(void*), void* ctx);
/**
* @brief Unsubscribes from external interrupt on the specified GPIO.
*
* This function is thread-safe with respect to all other public functions
* of this class, however it must not be called from an interrupt routine
* running at a higher priority than the interrupt that is being unsubscribed.
*/
void unsubscribe();
uint16_t get_pin_number() {
uint16_t pin_number = 0;
uint16_t pin_mask = pin_mask_ >> 1;
while (pin_mask) {
pin_mask >>= 1;
pin_number++;
}
return pin_number;
}
private:
GPIO_TypeDef* port_;
uint16_t pin_;
uint16_t pin_mask_; // TODO: store pin_number_ instead of pin_mask_
};
#endif // STM32_GPIO_HPP__
Firmware/MotorControl/axis.cpp
+7 -8
View File
@@ -146,22 +146,21 @@ void Axis::decode_step_dir_pins() {
// @brief (de)activates step/dir input
void Axis::set_step_dir_active(bool active) {
if (active) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
// Set up the step/direction GPIOs as input
Stm32Gpio{dir_port_, dir_pin_}.config(GPIO_MODE_INPUT, GPIO_NOPULL);
Stm32Gpio{step_port_, step_pin_}.config(GPIO_MODE_INPUT, GPIO_PULLDOWN);
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this);
Stm32Gpio{step_port_, step_pin_}.subscribe(true, false, step_cb_wrapper, this);
step_dir_active_ = true;
} else {
step_dir_active_ = false;
// Unsubscribe from step GPIO
GPIO_unsubscribe(step_port_, step_pin_);
// TODO: if we change the GPIO while the subscription is active and then
// unsubscribe then the unsubscribe is for the wrong pin.
Stm32Gpio{step_port_, step_pin_}.unsubscribe();
}
}
Firmware/MotorControl/encoder.cpp
+47 -20
View File
@@ -3,28 +3,36 @@
#include <Drivers/STM32/stm32_system.h>
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)
Encoder::Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
Stm32SpiArbiter* spi_arbiter) :
timer_(timer), index_gpio_(index_gpio),
hallA_gpio_(hallA_gpio), hallB_gpio_(hallB_gpio), hallC_gpio_(hallC_gpio),
spi_arbiter_(spi_arbiter)
{
update_pll_gains();
if (config.pre_calibrated) {
if (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)
is_ready_ = true;
if (motor_config.motor_type == Motor::MOTOR_TYPE_ACIM)
is_ready_ = true;
}
}
static void enc_index_cb_wrapper(void* ctx) {
reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
}
bool Encoder::apply_config(ODriveIntf::MotorIntf::MotorType motor_type) {
config_.parent = this;
update_pll_gains();
if (config_.pre_calibrated) {
if (config_.mode == Encoder::MODE_HALL || config_.mode == Encoder::MODE_SINCOS)
is_ready_ = true;
if (motor_type == Motor::MOTOR_TYPE_ACIM)
is_ready_ = true;
}
return true;
}
void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
set_idx_subscribe();
mode_ = config_.mode;
@@ -90,15 +98,15 @@ void Encoder::enc_index_cb() {
}
// Disable interrupt
GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
index_gpio_.unsubscribe();
}
void Encoder::set_idx_subscribe(bool override_enable) {
if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
enc_index_cb_wrapper, this);
index_gpio_.config(GPIO_MODE_INPUT, GPIO_PULLDOWN);
index_gpio_.subscribe(true, false, enc_index_cb_wrapper, this);
} else if (!config_.use_index || config_.find_idx_on_lockin_only) {
GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
index_gpio_.unsubscribe();
}
}
@@ -131,7 +139,7 @@ void Encoder::set_linear_count(int32_t count) {
tim_cnt_sample_ = count;
//Write hardware last
hw_config_.timer->Instance->CNT = count;
timer_->Instance->CNT = count;
cpu_exit_critical(prim);
}
@@ -313,7 +321,7 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
void Encoder::sample_now() {
switch (mode_) {
case MODE_INCREMENTAL: {
tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
tim_cnt_sample_ = (int16_t)timer_->Instance->CNT;
} break;
case MODE_HALL: {
@@ -337,6 +345,25 @@ void Encoder::sample_now() {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
} break;
}
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
port_samples_[i] = ports_to_sample[i]->IDR;
}
}
bool Encoder::read_sampled_gpio(Stm32Gpio gpio) {
for (size_t i = 0; i < sizeof(ports_to_sample) / sizeof(ports_to_sample[0]); ++i) {
if (ports_to_sample[i] == gpio.port_) {
return port_samples_[i] & gpio.pin_mask_;
}
}
return false;
}
void Encoder::decode_hall_samples() {
hall_state_ = (read_sampled_gpio(hallA_gpio_) ? 1 : 0)
| (read_sampled_gpio(hallB_gpio_) ? 2 : 0)
| (read_sampled_gpio(hallC_gpio_) ? 4 : 0);
}
bool Encoder::abs_spi_start_transaction(){
Firmware/MotorControl/encoder.hpp
+17 -5
View File
@@ -2,9 +2,9 @@
#define __ENCODER_HPP
#include <arm_math.h>
#include <Drivers/STM32/stm32_spi_arbiter.hpp>
#include "utils.hpp"
#include <autogen/interfaces.hpp>
class Encoder : public ODriveIntf::EncoderIntf {
public:
@@ -43,9 +43,11 @@ public:
void set_bandwidth(float value) { bandwidth = value; parent->update_pll_gains(); }
};
Encoder(const EncoderHardwareConfig_t& hw_config, Stm32SpiArbiter* spi_arbiter,
Config_t& config, const Motor::Config_t& motor_config);
Encoder(TIM_HandleTypeDef* timer, Stm32Gpio index_gpio,
Stm32Gpio hallA_gpio, Stm32Gpio hallB_gpio, Stm32Gpio hallC_gpio,
Stm32SpiArbiter* spi_arbiter);
bool apply_config(ODriveIntf::MotorIntf::MotorType motor_type);
void setup();
void set_error(Error error);
bool do_checks();
@@ -63,13 +65,20 @@ public:
bool run_direction_find();
bool run_offset_calibration();
void sample_now();
bool read_sampled_gpio(Stm32Gpio gpio);
void decode_hall_samples();
bool update();
const EncoderHardwareConfig_t& hw_config_;
TIM_HandleTypeDef* timer_;
Stm32Gpio index_gpio_;
Stm32Gpio hallA_gpio_;
Stm32Gpio hallB_gpio_;
Stm32Gpio hallC_gpio_;
Stm32SpiArbiter* spi_arbiter_;
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Config_t config_;
Error error_ = ERROR_NONE;
bool index_found_ = false;
bool is_ready_ = false;
@@ -90,6 +99,8 @@ public:
bool vel_estimate_valid_ = false;
int16_t tim_cnt_sample_ = 0; //
static const constexpr GPIO_TypeDef* ports_to_sample[] = { GPIOA, GPIOB, GPIOC };
uint16_t port_samples_[sizeof(ports_to_sample) / sizeof(ports_to_sample[0])];
// Updated by low_level pwm_adc_cb
uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
float sincos_sample_s_ = 0.0f;
@@ -112,6 +123,7 @@ public:
constexpr float getCoggingRatio(){
return config_.cpr / 3600.0f;
}
};
#endif // __ENCODER_HPP
Firmware/MotorControl/low_level.cpp
+3 -76
View File
@@ -39,12 +39,6 @@ float ibus_ = 0.0f; // exposed for monitoring only
bool brake_resistor_armed = false;
bool brake_resistor_saturated = false;
/* Private constant data -----------------------------------------------------*/
static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC };
static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]);
/* Private variables ---------------------------------------------------------*/
// Two motors, sampling port A,B,C (coherent with current meas timing)
static uint16_t GPIO_port_samples [2][num_GPIO];
/* CPU critical section helpers ----------------------------------------------*/
/* Safety critical functions -------------------------------------------------*/
@@ -374,8 +368,8 @@ void start_general_purpose_adc() {
}
// @brief Returns the ADC voltage associated with the specified pin.
// GPIO_set_to_analog() must be called first to put the Pin into
// analog mode.
// This only works if the GPIO was not used for anything else since bootup, otherwise
// it must be put to analog mode first.
// Returns NaN if the pin has no associated ADC1 channel.
//
// On ODrive 3.3 and 3.4 the following pins can be used with this function:
@@ -460,35 +454,6 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
vbus_voltage = ADCValue * voltage_scale;
}
static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
GPIO_TypeDef* hall_ports[] = {
enc.hw_config_.hallC_port,
enc.hw_config_.hallB_port,
enc.hw_config_.hallA_port,
};
uint16_t hall_pins[] = {
enc.hw_config_.hallC_pin,
enc.hw_config_.hallB_pin,
enc.hw_config_.hallA_pin,
};
uint8_t hall_state = 0x0;
for (int i = 0; i < 3; ++i) {
int port_idx = 0;
for (;;) {
auto port = GPIOs_to_samp[port_idx];
if (port == hall_ports[i])
break;
++port_idx;
}
hall_state <<= 1;
hall_state |= (GPIO_samples[port_idx] & hall_pins[i]) ? 1 : 0;
}
enc.hall_state_ = hall_state;
}
// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
// TODO: Document how the phasing is done, link to timing diagram
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
@@ -576,7 +541,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
}
// Prepare hall readings
// TODO move this to inside encoder update function
decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]);
axis.encoder_.decode_hall_samples();
// Trigger axis thread
axis.signal_current_meas();
} else {
@@ -589,34 +554,6 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
}
}
void tim_update_cb(TIM_HandleTypeDef* htim) {
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
// If we are counting down, we just sampled in SVM vector 7, with zero current
bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR;
if (counting_down)
return;
int sample_ch;
Axis* axis;
if (htim == &htim1) {
sample_ch = 0;
axis = axes[0];
} else if (htim == &htim8) {
sample_ch = 1;
axis = axes[1];
} else {
low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK);
return;
}
axis->encoder_.sample_now();
for (int i = 0; i < num_GPIO; ++i) {
GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR;
}
}
// @brief Sums up the Ibus contribution of each motor and updates the
// brake resistor PWM accordingly.
void update_brake_current() {
@@ -811,13 +748,3 @@ void start_analog_thread() {
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 512 / sizeof(StackType_t));
osThreadCreate(osThread(thread_def), NULL);
}
/*
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
if(hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_)
axes[0]->encoder_.abs_spi_cb();
else if (hspi->pRxBuffPtr == (uint8_t*)axes[1]->encoder_.abs_spi_dma_rx_)
axes[1]->encoder_.abs_spi_cb();
}
*/
Firmware/MotorControl/main.cpp
+35 -20
View File
@@ -11,7 +11,6 @@
#include <communication/interface_can.hpp>
ODriveCAN::Config_t can_config;
Encoder::Config_t encoder_configs[AXIS_COUNT];
SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
Controller::Config_t controller_configs[AXIS_COUNT];
OffboardThermistorCurrentLimiter::Config_t motor_thermistor_configs[AXIS_COUNT];
@@ -32,14 +31,14 @@ static bool config_pop_all() {
config_manager.pop(&odrv.config_) &&
config_manager.pop(&can_config);
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
success = config_manager.pop(&encoder_configs[i]) &&
success = config_manager.pop(&encoders[i].config_) &&
config_manager.pop(&sensorless_configs[i]) &&
config_manager.pop(&controller_configs[i]) &&
config_manager.pop(&trap_configs[i]) &&
config_manager.pop(&min_endstop_configs[i]) &&
config_manager.pop(&max_endstop_configs[i]) &&
config_manager.pop(&motors[i]->config_) &&
config_manager.pop(&fet_thermistors[i]->config_) &&
config_manager.pop(&motors[i].config_) &&
config_manager.pop(&fet_thermistors[i].config_) &&
config_manager.pop(&motor_thermistor_configs[i]) &&
config_manager.pop(&axis_configs[i]);
}
@@ -51,14 +50,14 @@ static bool config_push_all() {
config_manager.push(&odrv.config_) &&
config_manager.push(&can_config);
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
success = config_manager.push(&encoder_configs[i]) &&
success = config_manager.push(&encoders[i].config_) &&
config_manager.push(&sensorless_configs[i]) &&
config_manager.push(&controller_configs[i]) &&
config_manager.push(&trap_configs[i]) &&
config_manager.push(&min_endstop_configs[i]) &&
config_manager.push(&max_endstop_configs[i]) &&
config_manager.push(&motors[i]->config_) &&
config_manager.push(&fet_thermistors[i]->config_) &&
config_manager.push(&motors[i].config_) &&
config_manager.push(&fet_thermistors[i].config_) &&
config_manager.push(&motor_thermistor_configs[i]) &&
config_manager.push(&axis_configs[i]);
}
@@ -69,12 +68,12 @@ static void config_clear_all() {
odrv.config_ = {};
can_config = {};
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = {};
encoders[i].config_ = {};
sensorless_configs[i] = {};
controller_configs[i] = {};
trap_configs[i] = {};
motors[i]->config_ = {};
fet_thermistors[i]->config_ = {};
motors[i].config_ = {};
fet_thermistors[i].config_ = {};
axis_configs[i] = {};
// Default step/dir pins are different, so we need to explicitly load them
Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]);
@@ -88,7 +87,8 @@ static void config_clear_all() {
static bool config_apply_all() {
bool success = true;
for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
success = motors[i]->apply_config();
success = encoders[i].apply_config(motors[i].config_.motor_type)
&& motors[i].apply_config();
}
return success;
}
@@ -201,8 +201,6 @@ 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, &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]);
OffboardThermistorCurrentLimiter *motor_thermistor = new OffboardThermistorCurrentLimiter(motor_thermistor_configs[i]);
@@ -210,10 +208,9 @@ extern "C" int construct_objects(){
Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, i ? m1_fet_thermistor : m0_fet_thermistor, *motor_thermistor, i ? m1 : m0, *trap, *min_endstop, *max_endstop);
encoders[i], *sensorless_estimator, *controller, fet_thermistors[i], *motor_thermistor, motors[i], *trap, *min_endstop, *max_endstop);
controller_configs[i].parent = controller;
encoder_configs[i].parent = encoder;
motor_thermistor_configs[i].parent = motor_thermistor;
min_endstop_configs[i].parent = min_endstop;
max_endstop_configs[i].parent = max_endstop;
@@ -236,8 +233,9 @@ void vApplicationIdleHook(void) {
odrv.system_stats_.uptime = xTaskGetTickCount();
odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
odrv.system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
uint32_t min_stack_space[AXIS_COUNT];
std::transform(axes.begin(), axes.end(), std::begin(min_stack_space), [](auto& axis) { return uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t); });
odrv.system_stats_.min_stack_space_axis = *std::min_element(std::begin(min_stack_space), std::end(min_stack_space));
odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
@@ -245,8 +243,7 @@ void vApplicationIdleHook(void) {
odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
// Actual usage, in bytes, so we don't have to math
odrv.system_stats_.stack_usage_axis0 = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis0;
odrv.system_stats_.stack_usage_axis1 = axes[1]->stack_size_ - odrv.system_stats_.min_stack_space_axis1;
odrv.system_stats_.stack_usage_axis = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis;
odrv.system_stats_.stack_usage_comms = stack_size_comm_thread - odrv.system_stats_.min_stack_space_comms;
odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb;
odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart;
@@ -264,7 +261,25 @@ int odrive_main(void) {
// TODO: make dynamically reconfigurable
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (odrv.config_.enable_uart) {
SetGPIO12toUART();
GPIO_InitTypeDef GPIO_InitStruct;
// make sure nothing is hogging the GPIO's
Stm32Gpio{GPIO_1_GPIO_Port, GPIO_1_Pin}.unsubscribe();
Stm32Gpio{GPIO_2_GPIO_Port, GPIO_2_Pin}.unsubscribe();
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
}
#endif
//osDelay(100);
Firmware/MotorControl/motor.cpp
+10
View File
@@ -454,3 +454,13 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
}
return true;
}
void Motor::tim_update_cb() {
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
// If we are counting down, we just sampled in SVM vector 7, with zero current
bool counting_down = timer_->Instance->CR1 & TIM_CR1_DIR;
if (counting_down)
return;
axis_->encoder_.sample_now();
}
Firmware/MotorControl/motor.hpp
+4 -6
View File
@@ -6,9 +6,9 @@ class Motor;
#include <board.h>
#ifndef __TimingLog_t
#define __TimingLog_t
enum TimingLog_t { // TODO: remove
#include <autogen/interfaces.hpp>
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
@@ -23,9 +23,6 @@ enum TimingLog_t { // TODO: remove
TIMING_LOG_SPI_END,
TIMING_LOG_NUM_SLOTS
};
#endif
#include <autogen/interfaces.hpp>
class Motor : public ODriveIntf::MotorIntf {
public:
@@ -123,6 +120,7 @@ public:
bool FOC_voltage(float v_d, float v_q, float pwm_phase);
bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase);
bool update(float current_setpoint, float phase, float phase_vel);
void tim_update_cb();
// hardware config
Firmware/MotorControl/odrive_main.h
+2 -4
View File
@@ -35,8 +35,7 @@ typedef struct {
bool fully_booted;
uint32_t uptime; // [ms]
uint32_t min_heap_space; // FreeRTOS heap [Bytes]
uint32_t min_stack_space_axis0; // minimum remaining space since startup [Bytes]
uint32_t min_stack_space_axis1;
uint32_t min_stack_space_axis; // minimum remaining space since startup [Bytes]
uint32_t min_stack_space_comms;
uint32_t min_stack_space_usb;
uint32_t min_stack_space_uart;
@@ -44,8 +43,7 @@ typedef struct {
uint32_t min_stack_space_startup;
uint32_t min_stack_space_can;
uint32_t stack_usage_axis0;
uint32_t stack_usage_axis1;
uint32_t stack_usage_axis;
uint32_t stack_usage_comms;
uint32_t stack_usage_usb;
uint32_t stack_usage_uart;
Firmware/MotorControl/thermistor.hpp
-20
View File
@@ -4,26 +4,6 @@
class Axis; // declared in axis.hpp
#include "current_limiter.hpp"
#ifndef __TimingLog_t
#define __TimingLog_t
enum TimingLog_t { // TODO: remove
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_SPI_START,
TIMING_LOG_SAMPLE_NOW,
TIMING_LOG_SPI_END,
TIMING_LOG_NUM_SLOTS
};
#endif
#include <autogen/interfaces.hpp>
class ThermistorCurrentLimiter : public CurrentLimiter, public ODriveIntf::ThermistorCurrentLimiterIntf {
Firmware/Tupfile.lua
+1
View File
@@ -196,6 +196,7 @@ sources = {
'MotorControl/sensorless_estimator.cpp',
'MotorControl/trapTraj.cpp',
'MotorControl/main.cpp',
'Drivers/STM32/stm32_gpio.cpp',
'Drivers/STM32/stm32_spi_arbiter.cpp',
'communication/can_simple.cpp',
'communication/communication.cpp',
Firmware/odrive-interface.yaml
+14 -16
View File
@@ -51,16 +51,14 @@ interfaces:
attributes:
uptime: readonly uint32
min_heap_space: readonly uint32
min_stack_space_axis0: readonly uint32
min_stack_space_axis1: readonly uint32
min_stack_space_axis: readonly uint32
min_stack_space_comms: readonly uint32
min_stack_space_usb: readonly uint32
min_stack_space_uart: readonly uint32
min_stack_space_can: readonly uint32
min_stack_space_usb_irq: readonly uint32
min_stack_space_startup: readonly uint32
stack_usage_axis0: readonly uint32
stack_usage_axis1: readonly uint32
stack_usage_axis: readonly uint32
stack_usage_comms: readonly uint32
stack_usage_usb: readonly uint32
stack_usage_uart: readonly uint32
@@ -574,18 +572,18 @@ interfaces:
timing_log:
c_is_class: False
attributes:
general: {type: readonly uint16, c_name: 'get(TIMING_LOG_GENERAL)'}
adc_cb_i: {type: readonly uint16, c_name: 'get(TIMING_LOG_ADC_CB_I)'}
adc_cb_dc: {type: readonly uint16, c_name: 'get(TIMING_LOG_ADC_CB_DC)'}
meas_r: {type: readonly uint16, c_name: 'get(TIMING_LOG_MEAS_R)'}
meas_l: {type: readonly uint16, c_name: 'get(TIMING_LOG_MEAS_L)'}
enc_calib: {type: readonly uint16, c_name: 'get(TIMING_LOG_ENC_CALIB)'}
idx_search: {type: readonly uint16, c_name: 'get(TIMING_LOG_IDX_SEARCH)'}
foc_voltage: {type: readonly uint16, c_name: 'get(TIMING_LOG_FOC_VOLTAGE)'}
foc_current: {type: readonly uint16, c_name: 'get(TIMING_LOG_FOC_CURRENT)'}
spi_start: {type: readonly uint16, c_name: 'get(TIMING_LOG_SPI_START)'}
sample_now: {type: readonly uint16, c_name: 'get(TIMING_LOG_SAMPLE_NOW)'}
spi_end: {type: readonly uint16, c_name: 'get(TIMING_LOG_SPI_END)'}
general: {type: readonly uint16, c_name: 'get(0)'}
adc_cb_i: {type: readonly uint16, c_name: 'get(1)'}
adc_cb_dc: {type: readonly uint16, c_name: 'get(2)'}
meas_r: {type: readonly uint16, c_name: 'get(3)'}
meas_l: {type: readonly uint16, c_name: 'get(4)'}
enc_calib: {type: readonly uint16, c_name: 'get(5)'}
idx_search: {type: readonly uint16, c_name: 'get(6)'}
foc_voltage: {type: readonly uint16, c_name: 'get(7)'}
foc_current: {type: readonly uint16, c_name: 'get(8)'}
spi_start: {type: readonly uint16, c_name: 'get(9)'}
sample_now: {type: readonly uint16, c_name: 'get(10)'}
spi_end: {type: readonly uint16, c_name: 'get(11)'}
config:
c_is_class: False
attributes:
tools/odrive/tests/step_dir_test.py
+6
View File
@@ -43,6 +43,7 @@ class TestStepDir():
dir_gpio.config(output=True)
dir_gpio.write(True)
axis.parent.erase_config_and_reboot()
if axis.num == 0:
axis.parent.handle.config.enable_uart = False
axis.handle.config.enable_step_dir = True
@@ -52,6 +53,7 @@ class TestStepDir():
request_state(axis, AXIS_STATE_IDLE) # apply step_dir_always_on config
axis.handle.controller.input_pos = 0
ref = axis.handle.controller.input_pos
axis.handle.config.counts_per_step = counts_per_step = 10
@@ -59,6 +61,7 @@ class TestStepDir():
for i in range(100):
step_gpio.write(True)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
step_gpio.write(False)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
@@ -67,6 +70,7 @@ class TestStepDir():
for i in range(100):
step_gpio.write(True)
test_assert_eq(axis.handle.controller.input_pos, ref - (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
step_gpio.write(False)
test_assert_eq(axis.handle.controller.input_pos, ref - (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
@@ -76,6 +80,7 @@ class TestStepDir():
for i in range(100):
step_gpio.write(True)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
step_gpio.write(False)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * counts_per_step)
@@ -84,6 +89,7 @@ class TestStepDir():
for i in range(100):
step_gpio.write(True)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * abs(counts_per_step))
step_gpio.write(False)
test_assert_eq(axis.handle.controller.input_pos, ref + (i + 1) * counts_per_step, range = 0.4 * abs(counts_per_step))