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

@@ -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

Firmware/Board/v3/Inc/board.h

@@ -23,6 +23,9 @@
 #endif
 
 
+static const size_t AXIS_COUNT = 2;
+
+
 #ifdef __cplusplus
 #include <Drivers/STM32/stm32_gpio.hpp>
 #include <Drivers/DRV8301/drv8301.hpp>
@@ -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 <Drivers/STM32/stm32_spi_arbiter.hpp>
 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

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);

Firmware/MotorControl/main.cpp

@@ -24,85 +24,103 @@ std::array<Axis*, AXIS_COUNT> 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) {

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

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();
 

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)

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<Ts...>.
-//
-// Config<Ts...> has two template specializations to implement template recursion:
-// - Config<T, Ts...> handles loading/storing of the first object (type T) and leaves
-//   the rest of the objects to an "inner" class Config<Ts...>.
-// - Config<> represents the leaf of the recursion.
-template<typename ... Ts>
-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<typename T, typename ... Ts>
-struct Config<T, Ts...> {
-    static size_t get_size() {
-        return sizeof(T) + Config<Ts...>::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<typename T>
+    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<CONFIG_CRC16_POLYNOMIAL>(previous_crc16, (uint8_t *)val0, size);
-        if (Config<Ts...>::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<CONFIG_CRC16_POLYNOMIAL>(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<CONFIG_CRC16_POLYNOMIAL>(*crc16, (uint8_t *)val0, size);
-        if (Config<Ts...>::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<T, Ts..., uint16_t>::get_size() > NVM_get_max_read_length())
-            return -1;
-        uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
-        if (Config<T, Ts..., uint16_t>::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<T, Ts...>::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<T, Ts...>::store_config(0, &crc16, val0, vals...))
-            return -1;
-        if (Config<uint8_t, uint8_t>::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16))
-            return -1;
-        if (NVM_commit())
-            return -1;
-        return 0;
+    template<typename T>
+    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<CONFIG_CRC16_POLYNOMIAL>(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;
 };

Firmware/MotorControl/odrive_main.h

@@ -138,7 +138,6 @@ class Axis;
 class Motor;
 class ODriveCAN;
 
-constexpr size_t AXIS_COUNT = 2;
 extern std::array<Axis*, AXIS_COUNT> axes;
 extern ODriveCAN *odCAN;