From 2d256f43cbaafdee9a1add740411d7b305660e0a Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 7 Jul 2020 10:56:04 +0200 Subject: [PATCH] more refactoring in perparation for new hardware --- Firmware/Board/v3/Inc/board.h | 2 +- .../Class/CDC/Src/usbd_cdc.c | 2 +- Firmware/Board/v3/Src/usbd_desc.c | 1 - Firmware/Board/v3/board.cpp | 4 ++- Firmware/Drivers/DRV8301/drv8301.cpp | 36 +++++++++---------- Firmware/Drivers/DRV8301/drv8301.hpp | 12 ++++--- Firmware/Drivers/STM32/stm32_spi_arbiter.cpp | 20 ++++++----- Firmware/Drivers/STM32/stm32_system.h | 36 ++++++++++++++++++- Firmware/MotorControl/main.cpp | 24 ++++++++----- Firmware/MotorControl/motor.cpp | 4 ++- Firmware/MotorControl/nvm_config.hpp | 12 +++++-- Firmware/MotorControl/odrive_main.h | 2 +- Firmware/odrive-interface.yaml | 2 +- 13 files changed, 108 insertions(+), 49 deletions(-) diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 58b2d31b..d6271e95 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -120,6 +120,6 @@ static inline void board_clear_config() { } static inline bool board_apply_config() { return true; } void system_init(); -void board_init(); +bool board_init(); #endif // __BOARD_CONFIG_H diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index a3042278..7fabf6ee 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -327,7 +327,7 @@ __ALIGN_BEGIN uint8_t USBD_CDC_CfgDesc[USB_CDC_CONFIG_DESC_SIZ] __ALIGN_END = LOBYTE(CDC_DATA_HS_MAX_PACKET_SIZE), /* wMaxPacketSize: */ HIBYTE(CDC_DATA_HS_MAX_PACKET_SIZE), 0x00, /* bInterval: ignore for Bulk transfer */ -} ; +}; /** diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index d213f64a..299ed329 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -99,7 +99,6 @@ #define USBD_PRODUCT_STR(s) #s #define USBD_PRODUCT_STRING_FS ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR CDC Interface #define NATIVE_STRING ODrive HW_VERSION_MAJOR.HW_VERSION_MINOR Native Interface -#define USBD_SERIALNUMBER_STRING_FS "000000000001" #define USBD_CONFIGURATION_STRING_FS "CDC Config" #define USBD_INTERFACE_STRING_FS "CDC Interface" diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index d636dc25..429ebd84 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -261,7 +261,7 @@ void system_init() { SystemClock_Config(); } -void board_init() { +bool board_init() { // Initialize all configured peripherals MX_GPIO_Init(); MX_DMA_Init(); @@ -331,6 +331,8 @@ void board_init() { {&htim1, &htim8, &htim13}, {TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128 /* TODO: explain why this offset */, 0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128} ); + + return true; } diff --git a/Firmware/Drivers/DRV8301/drv8301.cpp b/Firmware/Drivers/DRV8301/drv8301.cpp index a8635c4e..e2fb20c2 100644 --- a/Firmware/Drivers/DRV8301/drv8301.cpp +++ b/Firmware/Drivers/DRV8301/drv8301.cpp @@ -59,6 +59,23 @@ const SPI_InitTypeDef Drv8301::spi_config_ = { .CRCPolynomial = 10, }; +bool Drv8301::init() { + enable_gpio_.write(true); + + // Wait for driver to come online + osDelay(10); + + // Make sure the Fault bit is not set during startup + uint16_t reg; + while (!read_spi(RegName_Status_1, ®) || (reg & DRV8301_STATUS1_FAULT_BITS)) + ; // TODO: don't spin + + // Wait for the DRV8301 registers to update + osDelay(1); + + return true; +} + Drv8301::FaultType_e Drv8301::get_error() { uint16_t readWord; FaultType_e faultType = FaultType_NoFault; @@ -142,25 +159,6 @@ bool Drv8301::check_fault() { } } -bool Drv8301::set_enabled(bool enabled) { - enable_gpio_.write(enabled); - - if (enabled) { - // Wait for driver to come online - osDelay(10); - - // Make sure the Fault bit is not set during startup - uint16_t reg; - while (!read_spi(RegName_Status_1, ®) || (reg & DRV8301_STATUS1_FAULT_BITS)) - ; // TODO: don't spin - - // Wait for the DRV8301 registers to update - osDelay(1); - } - - return true; -} - bool Drv8301::read_spi(const RegName_e regName, uint16_t* data) { tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Read, regName, 0); if (!spi_arbiter_->transfer(spi_config_, ncs_gpio_, (uint8_t *)(&tx_buf_), nullptr, 1, 1000)) { diff --git a/Firmware/Drivers/DRV8301/drv8301.hpp b/Firmware/Drivers/DRV8301/drv8301.hpp index 155a6d41..cf0cc847 100644 --- a/Firmware/Drivers/DRV8301/drv8301.hpp +++ b/Firmware/Drivers/DRV8301/drv8301.hpp @@ -205,9 +205,16 @@ public: : spi_arbiter_(spi_arbiter), ncs_gpio_(ncs_gpio), enable_gpio_(enable_gpio), nfault_gpio_(nfault_gpio) {} + /** + * @brief Initializes the gate driver to a hardcoded default configuration. + * Returns true on success or false otherwise (e.g. if the gate driver is + * not connected). + */ + bool init(); + bool set_gain(float requested_gain, float* actual_gain) final; bool check_fault() final; - bool set_enabled(bool enabled) final; + bool set_enabled(bool enabled) final { return true; } FaultType_e get_error(); float get_midpoint() final { @@ -381,9 +388,6 @@ private: return ctrlMode | regName | (data & DRV8301_DATA_MASK); } // end of DRV8301_buildCtrlWord() function - //! \brief Enables the DRV8301 - void enable(); - //! \brief Reads data from the DRV8301 register //! \param[in] regName The register name //! \return The data value diff --git a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp index 8ee1a3a2..059799b7 100644 --- a/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp +++ b/Firmware/Drivers/STM32/stm32_spi_arbiter.cpp @@ -2,6 +2,7 @@ #include "stm32_spi_arbiter.hpp" #include "stm32_system.h" #include "utils.hpp" +#include bool equals(const SPI_InitTypeDef& lhs, const SPI_InitTypeDef& rhs) { return (lhs.Mode == rhs.Mode) @@ -47,15 +48,15 @@ bool Stm32SpiArbiter::start() { } void Stm32SpiArbiter::transfer_async(SpiTask* task) { + task->next = nullptr; + // 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_; - { - uint32_t prim = cpu_enter_critical(); + CRITICAL_SECTION() { while (*ptr) ptr = &(*ptr)->next; *ptr = task; - cpu_exit_critical(prim); } // If the list was empty before, kick off the SPI arbiter now @@ -68,8 +69,9 @@ void Stm32SpiArbiter::transfer_async(SpiTask* task) { } } +// TODO: this currently only works when called in a CMSIS thread. 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; + volatile uint8_t result = 0xff; SpiTask task = { .config = config, @@ -77,8 +79,8 @@ bool Stm32SpiArbiter::transfer(SPI_InitTypeDef config, Stm32Gpio ncs_gpio, const .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, + .on_complete = [](void* ctx, bool success) { *(volatile uint8_t*)ctx = success ? 1 : 0; }, + .cb_ctx = (void*)&result, .next = nullptr }; @@ -103,8 +105,10 @@ void Stm32SpiArbiter::on_complete() { } // Start next task if any - SpiTask* next = task_list_->next; - task_list_ = next; + SpiTask* next = nullptr; + CRITICAL_SECTION() { + next = task_list_ = task_list_->next; + } if (next) { start(); } diff --git a/Firmware/Drivers/STM32/stm32_system.h b/Firmware/Drivers/STM32/stm32_system.h index cdba1b77..ce8cae32 100644 --- a/Firmware/Drivers/STM32/stm32_system.h +++ b/Firmware/Drivers/STM32/stm32_system.h @@ -1,7 +1,17 @@ #ifndef __STM32_SYSTEM_H #define __STM32_SYSTEM_H -#include +#if defined(STM32F405xx) +#include +#else +#error "unknown STM32 microcontroller" +#endif + +// C/C++ definitions + +#ifdef __cplusplus +extern "C" { +#endif static inline uint32_t cpu_enter_critical() { uint32_t primask = __get_PRIMASK(); @@ -13,4 +23,28 @@ static inline void cpu_exit_critical(uint32_t priority_mask) { __set_PRIMASK(priority_mask); } +#ifdef __cplusplus +} +#endif + + +// C++ only definitions + +#ifdef __cplusplus + +struct CriticalSectionContext { + CriticalSectionContext(const CriticalSectionContext&) = delete; + CriticalSectionContext(const CriticalSectionContext&&) = delete; + void operator=(const CriticalSectionContext&) = delete; + void operator=(const CriticalSectionContext&&) = delete; + CriticalSectionContext() : mask_(cpu_enter_critical()) {} + ~CriticalSectionContext() { cpu_exit_critical(mask_); } + uint32_t mask_; + bool exit_ = false; +}; + +#define CRITICAL_SECTION() for (CriticalSectionContext __critical_section_context; !__critical_section_context.exit_; __critical_section_context.exit_ = true) + +#endif + #endif // __STM32_SYSTEM_H \ No newline at end of file diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index e91adf1f..fc6520d4 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -108,13 +108,14 @@ static bool config_apply_all() { } void ODrive::save_configuration(void) { + size_t config_size = 0; bool success = config_manager.prepare_store() && config_push_all() - && config_manager.start_store() + && config_manager.start_store(&config_size) && config_push_all() && config_manager.finish_store(); if (success) { - user_config_loaded_ = true; + user_config_loaded_ = config_size; } else { printf("saving configuration failed\r\n"); osDelay(5); @@ -220,10 +221,12 @@ static void rtos_main(void*) { // must happen after communication is initialized pwm0_input.init(); - // Setup hardware for all components + // Set up hardware for all components for (size_t i = 0; i < AXIS_COUNT; ++i) { if (!axes[i].setup()) { - for (;;); // TODO: proper error handling + for (;;) { + osDelay(10); // TODO: proper error handling + } } } @@ -331,11 +334,14 @@ extern "C" int main(void) { // Load configuration from NVM. This needs to happen after system_init() // since the flash interface must be initialized and before board_init() // since board initialization can depend on the config. - odrv.user_config_loaded_ = config_manager.start_load() + size_t config_size = 0; + bool success = config_manager.start_load() && config_pop_all() - && config_manager.finish_load() + && config_manager.finish_load(&config_size) && config_apply_all(); - if (!odrv.user_config_loaded_) { + if (success) { + odrv.user_config_loaded_ = config_size; + } else { config_clear_all(); config_apply_all(); } @@ -346,7 +352,9 @@ extern "C" int main(void) { || (odrv.config_.enable_uart2 && !uart2); // Init board-specific peripherals - board_init(); + if (!board_init()) { + for (;;); // TODO: handle properly + } // Init GPIOs according to their configured mode for (size_t i = 0; i < GPIO_COUNT; ++i) { diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 7cf380f4..05497c9c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -70,7 +70,9 @@ bool Motor::apply_config() { // @brief Set up the gate drivers bool Motor::setup() { - gate_driver_.set_enabled(true); + if (!gate_driver_.init()) { + return false; + } // Solve for exact gain, then snap down to have equal or larger range as requested // or largest possible range otherwise diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index 957da707..7ffa8ea4 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -89,7 +89,11 @@ public: * If this function returns false, it is possible that previous pop() * operations actually returned garbage. */ - bool finish_load() { + bool finish_load(size_t* occupied_size) { + if (occupied_size) { + *occupied_size = load_offset + 2; + } + uint16_t crc16_calculated = load_crc16; uint16_t crc16_loaded; if (!pop(&crc16_loaded)) { @@ -131,7 +135,11 @@ public: /** * @brief Finishes the prepare pass and starts the actual store pass. */ - bool start_store() { + bool start_store(size_t* occupied_size) { + if (occupied_size) { + *occupied_size = store_offset + 2; + } + if (store_state != kStoreStatePreparing) { return (store_state = kStoreStateFailed), false; } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index c4ba1399..d3a5ec6b 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -220,7 +220,7 @@ public: SystemStats_t system_stats_; BoardConfig_t config_; - bool user_config_loaded_; + uint32_t user_config_loaded_ = 0; bool misconfigured_ = false; uint32_t test_property_ = 0; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index ef43a496..37ec69e0 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -207,7 +207,7 @@ interfaces: gpio4_pwm_mapping: {type: Endpoint, c_name: 'pwm_mappings[3]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_PWM0`.} gpio3_analog_mapping: {type: Endpoint, c_name: 'analog_mappings[3]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_ANALOG_IN`.} gpio4_analog_mapping: {type: Endpoint, c_name: 'analog_mappings[4]', doc: Make sure the corresponding GPIO is in `GPIO_MODE_ANALOG_IN`.} - user_config_loaded: readonly bool + user_config_loaded: readonly uint32 misconfigured: type: readonly bool doc: |