From ab7957f07f3bbb9a09d3048a96bb506c18313d25 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 17 Jun 2020 22:16:57 +0200 Subject: [PATCH] Overhaul upper level NVM configuration manager Instead of using a declarative compile-time type that defines the layout of the NVM data, now we use procedural style code to load and store the configuration as a series of pop and push calls. This allows for more flexible code organization and opens the future possibility of storing dynamically sized data (e.g. an array prepended by a length field). --- CHANGELOG.md | 2 + Firmware/Board/v3/Inc/board.h | 12 ++ Firmware/Board/v3/board.cpp | 3 + Firmware/MotorControl/main.cpp | 161 +++++++++--------- Firmware/MotorControl/motor.cpp | 5 +- Firmware/MotorControl/motor.hpp | 2 +- Firmware/MotorControl/nvm.c | 1 + Firmware/MotorControl/nvm_config.hpp | 236 ++++++++++++++++----------- Firmware/MotorControl/odrive_main.h | 1 - 9 files changed, 250 insertions(+), 173 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 29223f0e..f6e02256 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -45,6 +45,8 @@ Please add a note of your changes below this heading if you make a Pull Request. * `axis.motor.thermal_current_lim` has been removed. Instead a new property is available `axis.motor.effective_current_lim` which contains the effective current limit including any thermal limits. * `axis.motor.get_inverter_temp()`, `axis.motor.inverter_temp_limit_lower` and `axis.motor.inverter_temp_limit_upper` have been moved to seperate fet thermistor object under `axis.fet_thermistor`. `get_inverter_temp()` function has been renamed to `temp` and is now a read-only property. * 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 # Releases ## [0.4.12] - 2020-05-06 diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index d845204b..7c930fb7 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -23,6 +23,9 @@ #endif +static const size_t AXIS_COUNT = 2; + + #ifdef __cplusplus #include #include @@ -37,6 +40,8 @@ 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]; #include extern Stm32SpiArbiter& ext_spi_arbiter; @@ -139,4 +144,11 @@ const BoardHardwareConfig_t hw_configs[2] = { { #define I2C_A2_PORT GPIO_5_GPIO_Port #define I2C_A2_PIN GPIO_5_Pin +// This board has no board-specific user configurations +static inline bool board_pop_config() { return true; } +static inline bool board_push_config() { return true; } +static inline void board_clear_config() { } +static inline bool board_apply_config() { return true; } + + #endif // __BOARD_CONFIG_H diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index a63edf3a..59812a30 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -57,6 +57,9 @@ Motor m1{ m1_gate_driver // opamp }; +Motor* motors[AXIS_COUNT] = {&m0, &m1}; +OnboardThermistorCurrentLimiter* fet_thermistors[AXIS_COUNT] = {&m0_fet_thermistor, &m1_fet_thermistor}; + void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { HAL_SPI_TxRxCpltCallback(hspi); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index c5459a8e..209f90aa 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -24,85 +24,103 @@ std::array axes; ODriveCAN *odCAN = nullptr; ODrive odrv{}; -typedef Config< - BoardConfig_t, - ODriveCAN::Config_t, - Encoder::Config_t[AXIS_COUNT], - SensorlessEstimator::Config_t[AXIS_COUNT], - Controller::Config_t[AXIS_COUNT], - Motor::Config_t, Motor::Config_t, - OnboardThermistorCurrentLimiter::Config_t, OnboardThermistorCurrentLimiter::Config_t, - OffboardThermistorCurrentLimiter::Config_t[AXIS_COUNT], - TrapezoidalTrajectory::Config_t[AXIS_COUNT], - Endstop::Config_t[AXIS_COUNT], - Endstop::Config_t[AXIS_COUNT], - Axis::Config_t[AXIS_COUNT]> ConfigFormat; + +ConfigManager config_manager; + +static bool config_pop_all() { + bool success = board_pop_config() && + 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]) && + 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(&motor_thermistor_configs[i]) && + config_manager.pop(&axis_configs[i]); + } + return success; +} + +static bool config_push_all() { + bool success = board_push_config() && + 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]) && + 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(&motor_thermistor_configs[i]) && + config_manager.push(&axis_configs[i]); + } + return success; +} + +static void config_clear_all() { + odrv.config_ = {}; + can_config = {}; + for (size_t i = 0; i < AXIS_COUNT; ++i) { + encoder_configs[i] = {}; + sensorless_configs[i] = {}; + controller_configs[i] = {}; + trap_configs[i] = {}; + 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]); + Axis::load_default_can_id(i, axis_configs[i]); + min_endstop_configs[i] = {}; + max_endstop_configs[i] = {}; + controller_configs[i].load_encoder_axis = i; + } +} + +static bool config_apply_all() { + bool success = true; + for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { + success = motors[i]->apply_config(); + } + return success; +} + + void ODrive::save_configuration(void) { - if (ConfigFormat::safe_store_config( - &odrv.config_, - &can_config, - &encoder_configs, - &sensorless_configs, - &controller_configs, - &m0.config_, - &m1.config_, - &m0_fet_thermistor.config_, - &m1_fet_thermistor.config_, - &motor_thermistor_configs, - &trap_configs, - &min_endstop_configs, - &max_endstop_configs, - &axis_configs)) { - printf("saving configuration failed\r\n"); osDelay(5); + bool success = config_manager.prepare_store() + && config_push_all() + && config_manager.start_store() + && config_push_all() + && config_manager.finish_store(); + if (success) { + user_config_loaded_ = true; } else { - odrv.user_config_loaded_ = true; + printf("saving configuration failed\r\n"); + osDelay(5); } } extern "C" int load_configuration(void) { - // Try to load configs - if (NVM_init() || - ConfigFormat::safe_load_config( - &odrv.config_, - &can_config, - &encoder_configs, - &sensorless_configs, - &controller_configs, - &m0.config_, - &m1.config_, - &m0_fet_thermistor.config_, - &m1_fet_thermistor.config_, - &motor_thermistor_configs, - &trap_configs, - &min_endstop_configs, - &max_endstop_configs, - &axis_configs)) { - //If loading failed, restore defaults - odrv.config_ = BoardConfig_t(); - can_config = ODriveCAN::Config_t(); - m0.config_ = Motor::Config_t(); - m1.config_ = Motor::Config_t(); - m0_fet_thermistor.config_ = OnboardThermistorCurrentLimiter::Config_t(); - m1_fet_thermistor.config_ = OnboardThermistorCurrentLimiter::Config_t(); - for (size_t i = 0; i < AXIS_COUNT; ++i) { - encoder_configs[i] = Encoder::Config_t(); - sensorless_configs[i] = SensorlessEstimator::Config_t(); - controller_configs[i] = Controller::Config_t(); - motor_thermistor_configs[i] = OffboardThermistorCurrentLimiter::Config_t(); - trap_configs[i] = TrapezoidalTrajectory::Config_t(); - axis_configs[i] = Axis::Config_t(); - // 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]); - Axis::load_default_can_id(i, axis_configs[i]); - min_endstop_configs[i] = Endstop::Config_t(); - max_endstop_configs[i] = Endstop::Config_t(); - controller_configs[i].load_encoder_axis = i; - } - } else { + bool success = config_manager.start_load() + && config_pop_all() + && config_manager.finish_load() + && config_apply_all(); + if (success) { odrv.user_config_loaded_ = true; + } else { + config_clear_all(); + config_apply_all(); } - return odrv.user_config_loaded_; + return success ? 0 : -1; } void ODrive::erase_configuration(void) { @@ -180,9 +198,6 @@ extern "C" int construct_objects(){ HAL_GPIO_Init(GPIO_5_GPIO_Port, &GPIO_InitStruct); #endif - m0.reload_config(); - m1.reload_config(); - // Construct all objects. odCAN = new ODriveCAN(can_config, &hcan1); for (size_t i = 0; i < AXIS_COUNT; ++i) { diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 89d01b1d..5d609175 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -16,7 +16,7 @@ Motor::Motor(TIM_HandleTypeDef* timer, shunt_conductance_(shunt_conductance), gate_driver_(gate_driver), opamp_(opamp) { - reload_config(); + apply_config(); } // @brief Arms the PWM outputs that belong to this motor. @@ -61,10 +61,11 @@ void Motor::update_current_controller_gains() { current_control_.i_gain = plant_pole * current_control_.p_gain; } -void Motor::reload_config() { +bool Motor::apply_config() { config_.parent = this; is_calibrated_ = config_.pre_calibrated; update_current_controller_gains(); + return true; } // @brief Set up the gate drivers diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 991a4cab..a3b910f0 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -104,7 +104,7 @@ public: bool arm(); void disarm(); - void reload_config(); + bool apply_config(); bool setup(); void reset_current_control(); diff --git a/Firmware/MotorControl/nvm.c b/Firmware/MotorControl/nvm.c index 5a1b4a71..b21a0148 100644 --- a/Firmware/MotorControl/nvm.c +++ b/Firmware/MotorControl/nvm.c @@ -262,6 +262,7 @@ size_t NVM_get_max_write_length(void) { } // @brief Reads from the latest committed block in the non-volatile memory. +// The function either succeeds or leaves the provided buffer unmodified. // @param offset: offset in bytes (0 meaning the beginning of the valid area) // @param data: buffer to write to // @param length: length in bytes (if (offset + length) is out of range, the function fails) diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index 2a96f595..7d48b5ef 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -26,116 +26,160 @@ /* Private constant data -----------------------------------------------------*/ // IMPORTANT: if you change, reorder or otherwise modify any of the fields in -// the config structs, make sure to increment this number: +// the config structs without changing its total length, make sure to increment this number: static constexpr uint16_t config_version = 0x0001; /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ - -// @brief Manages configuration load and store operations from and to NVM -// -// The NVM stores consecutive one-to-one copies of arbitrary objects. -// The types of these objects are passed as template arguments to Config. -// -// Config has two template specializations to implement template recursion: -// - Config handles loading/storing of the first object (type T) and leaves -// the rest of the objects to an "inner" class Config. -// - Config<> represents the leaf of the recursion. -template -struct Config; - -template<> -struct Config<> { - static size_t get_size() { - return 0; - } - static int load_config(size_t offset, uint16_t* crc16) { - return 0; - } - static int store_config(size_t offset, uint16_t* crc16) { - return 0; - } -}; - -template -struct Config { - static size_t get_size() { - return sizeof(T) + Config::get_size(); +/** + * @brief Manages configuration load and store operations from and to NVM + * + * Usage: + * 1. start_load() + * 2. pop() (as often needed) + * 3. finish_load() (to see if all pops were successful and the CRC in the end is valid) + * + * 1. prepare_store() + * 2. push() (as often as needed) + * 3. start_store() + * 4. push() (same sequence as before) + * 5. finish_store() + * + * The two store passes are required in order to measure the size on the first + * pass. If the size increases between the first and second pass, finish_store() + * will return an error. + */ +class ConfigManager { +public: + /** + * @brief Starts a load operation. This can be called at any time, even half + * way through a previous load operation. + */ + bool start_load() { + if (NVM_init() != 0) { + return (load_state = kLoadStateFailed), false; + } + load_offset = 0; + load_crc16 = CONFIG_CRC16_INIT ^ config_version; + load_state = kLoadStateInProgress; + return true; } - // @brief Loads one or more consecutive objects from the NVM. - // During loading this function also calculates the CRC over the loaded data. - // @param offset: 0 means that the function should start reading at the beginning - // of the last comitted NVM block - // @param crc16: the result of the CRC calculation is written to this address - // @param val0, vals: the values to be loaded - static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) { + /** + * @brief Loads the next chunk from NVM. + * Note that this may return true even if invalid data was read. The user + * will know the final verdict by the return value of finish_load(). + */ + template + bool pop(T* val) { + if (load_state != 1) { + return (load_state = kLoadStateFailed), false; + } size_t size = sizeof(T); - // save current CRC (in case val0 and crc16 point to the same address) - size_t previous_crc16 = *crc16; - if (NVM_read(offset, (uint8_t *)val0, size)) - return -1; - *crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size); - if (Config::load_config(offset + size, crc16, vals...)) - return -1; - return 0; + if (NVM_read(load_offset, (uint8_t *)val, size) != 0) + return (load_state = kLoadStateFailed), false; + load_crc16 = calc_crc16(load_crc16, (uint8_t *)val, size); + load_offset += size; + return true; } - // @brief Stores one or more consecutive objects to the NVM. - // During storing this function also calculates the CRC over the stored data. - // @param offset: 0 means that the function should start writing at the beginning - // of the currently active NVM write block - // @param crc16: the result of the CRC calculation is written to this address - // @param val0, vals: the values to be stored - static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) { - size_t size = sizeof(T); - if (NVM_write(offset, (uint8_t *)val0, size)) - return -1; - // update CRC _after_ writing (in case val0 and crc16 point to the same address) - if (crc16) - *crc16 = calc_crc16(*crc16, (uint8_t *)val0, size); - if (Config::store_config(offset + size, crc16, vals...)) - return -1; - return 0; + /** + * @brief Checks the final state of the load operation. + * If this function returns false, it is possible that previous pop() + * operations actually returned garbage. + */ + bool finish_load() { + uint16_t crc16_calculated = load_crc16; + uint16_t crc16_loaded; + if (!pop(&crc16_loaded)) { + return (load_state = kLoadStateFailed), false; + } + bool result = (load_state == 1) && (crc16_loaded == crc16_calculated); + load_state = kLoadStateIdle; + return result; } - // @brief Loads one or more consecutive objects from the NVM. The loaded data - // is validated using a CRC value that is stored at the beginning of the data. - static int safe_load_config(T* val0, Ts* ... vals) { - //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); - if (Config::get_size() > NVM_get_max_read_length()) - return -1; - uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; - if (Config::load_config(0, &crc16, val0, vals..., &crc16)) - return -1; - if (crc16) - return -1; - return 0; + /** + * @brief Starts preparation of a new store operation. + */ + bool prepare_store() { + if (store_state != kStoreStateIdle) { + // it might be possible to restart the store process from other states but let's be safe + return (store_state = kStoreStateFailed), false; + } + store_offset = 0; + store_crc16 = CONFIG_CRC16_INIT ^ config_version; + store_state = kStoreStatePreparing; + return true; } - // @brief Stores one or more consecutive objects to the NVM. In addition to the - // provided objects, a CRC of the data is stored. - // - // The CRC includes a version number and thus adds some protection against - // changes of the config structs during firmware update. Note that if the total - // config data length changes, the CRC validation will fail even if the developer - // forgets to update the config version number. - static int safe_store_config(const T* val0, const Ts* ... vals) { - size_t size = Config::get_size() + 2; - //printf("config is %d bytes\r\n", size); osDelay(5); - if (size > NVM_get_max_write_length()) - return -1; - if (NVM_start_write(size)) - return -1; - uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; - if (Config::store_config(0, &crc16, val0, vals...)) - return -1; - if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) - return -1; - if (NVM_commit()) - return -1; - return 0; + template + bool push(T* val) { + if (store_state == kStoreStateInProgress) { + if (NVM_write(store_offset, (uint8_t*)val, sizeof(T)) != 0) { + return (store_state = kStoreStateFailed), false; + } + } else if (store_state != kStoreStatePreparing) { + return (store_state = kStoreStateFailed), false; + } + store_crc16 = calc_crc16(store_crc16, (uint8_t *)val, sizeof(T)); + store_offset += sizeof(T); + return true; } + + /** + * @brief Finishes the prepare pass and starts the actual store pass. + */ + bool start_store() { + if (store_state != kStoreStatePreparing) { + return (store_state = kStoreStateFailed), false; + } + store_offset += 2; // account for CRC16 + if (store_offset > NVM_get_max_write_length()) { + return (store_state = kStoreStateFailed), false; + } + if (NVM_start_write(store_offset) != 0) { + return (store_state = kStoreStateFailed), false; + } + store_offset = 0; + store_crc16 = CONFIG_CRC16_INIT ^ config_version; + store_state = kStoreStateInProgress; + return true; + } + + /** + * @brief Commits the store operation. + * If this function succeeds, the new configuration was successfully saved. + * If this function fails, the old configuration was not touched. + */ + bool finish_store() { + uint16_t crc16 = store_crc16; + if (!push(&crc16)) { + return (store_state = kStoreStateFailed), false; + } + if (NVM_commit() != 0) { + return (store_state = kStoreStateFailed), false; + } + store_state = kStoreStateIdle; + return true; + } + + enum { + kLoadStateIdle = 0, + kLoadStateInProgress = 1, + kLoadStateFailed = 2 + } load_state = kLoadStateIdle; + size_t load_offset; + size_t load_crc16; + + enum { + kStoreStateIdle = 0, + kStoreStatePreparing = 1, + kStoreStateInProgress = 2, + kStoreStateFailed = 3 + } store_state = kStoreStateIdle; + size_t store_offset; + size_t store_crc16; }; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 8d768bc8..7e211eae 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -138,7 +138,6 @@ class Axis; class Motor; class ODriveCAN; -constexpr size_t AXIS_COUNT = 2; extern std::array axes; extern ODriveCAN *odCAN;