Add persistent storage of certain motor parameters.

The persistent storage can be controlled through the "save_config" and "reset" RPC functions. If a valid configuration is present, it's automatically loaded on startup. For now only a few parameters are saved (see config.cpp).

Firmware/Makefile

@@ -85,6 +85,7 @@ C_SOURCES = \
   MotorControl/low_level.c
 CPP_SOURCES = \
   MotorControl/axis.cpp \
+  MotorControl/config.cpp \
   MotorControl/commands.cpp \
   MotorControl/protocol.cpp
 ASM_SOURCES = \

Firmware/MotorControl/commands.cpp

@@ -11,6 +11,7 @@
 #include "protocol.hpp"
 #include "freertos_vars.h"
 #include "utils.h"
+#include "config.h"
 
 #ifdef ENABLE_LEGACY_PROTOCOL
 #include "legacy_commands.h"
@@ -210,7 +211,13 @@ const Endpoint endpoints[] = {
         Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func),
             Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint),
         Endpoint::close_tree(),
-    Endpoint::close_tree() // motor1
+    Endpoint::close_tree(), // motor1
+    Endpoint::make_function("save_config", &save_configuration),
+        // no arguments
+    Endpoint::close_tree(),
+    Endpoint::make_function("reset", &reset),
+        // no arguments
+    Endpoint::close_tree()
 };
 // clang-format on
 

Firmware/MotorControl/config.cpp

@@ -0,0 +1,231 @@
+
+/* Includes ------------------------------------------------------------------*/
+
+#include "config.h"
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stm32f405xx.h>
+
+#include "nvm.h"
+#include "crc.hpp"
+#include "low_level.h"
+
+/* Private defines -----------------------------------------------------------*/
+#define CRC16_INIT 0xabcd
+
+#if HW_VERSION_MAJOR == 3
+#if HW_VERSION_MINOR <= 3
+#define SHUNT_RESISTANCE (675e-6f)
+#else
+#define SHUNT_RESISTANCE (500e-6f)
+#endif
+#endif
+
+/* Private macros ------------------------------------------------------------*/
+/* Private typedef -----------------------------------------------------------*/
+
+typedef struct {
+    Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL;  //see: Motor_control_mode_t
+    bool enable_step_dir = false;                    //auto enabled after calibration
+    float counts_per_step = 2.0f;
+    float pos_setpoint = 0.0f;
+    float pos_gain = 20.0f;  // [(counts/s) / counts]
+    float vel_setpoint = 0.0f;
+    //float vel_setpoint = 800.0f; <sensorless example>
+    float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
+    //float vel_gain = 15.0f / 200.0f; // [A/(rad/s)] <sensorless example>
+    float vel_integrator_gain = 10.0f / 10000.0f;  // [A/(counts/s * s)]
+    //float vel_integrator_gain = 0.0f; // [A/(rad/s * s)] <sensorless example>
+    float vel_integrator_current = 0.0f;  // [A]
+    float vel_limit = 20000.0f;           // [counts/s]
+    float current_setpoint = 0.0f;        // [A]
+    float calibration_current = 10.0f;    // [A]
+    float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
+    float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
+    float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
+    Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
+    //Motor_type_t motor_type = MOTOR_TYPE_GIMBAL;
+    float shunt_conductance = 1.0f / SHUNT_RESISTANCE;  //[S]
+    float phase_current_rev_gain = 0.0f;                // to be set by DRV8301_setup
+    Current_control_t current_control = {
+            // Read out max_allowed_current to see max supported value for current_lim.
+            // You can change DRV8301_ShuntAmpGain to get a different range.
+            // .current_lim = 75.0f, //[A]
+            .current_lim = 10.0f,  //[A]
+            .p_gain = 0.0f,        // [V/A] should be auto set after resistance and inductance measurement
+            .i_gain = 0.0f,        // [V/As] should be auto set after resistance and inductance measurement
+            .v_current_control_integral_d = 0.0f,
+            .v_current_control_integral_q = 0.0f,
+            .Ibus = 0.0f,
+            .final_v_alpha = 0.0f,
+            .final_v_beta = 0.0f,
+            .Iq_setpoint = 0.0f,
+            .Iq_measured = 0.0f,
+            .max_allowed_current = 0.0f,
+        };
+    Rotor_mode_t rotor_mode = ROTOR_MODE_ENCODER;
+} MotorConfig_t;
+
+/* Global constant data ------------------------------------------------------*/
+/* Global variables ----------------------------------------------------------*/
+/* Private constant data -----------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+/* Private function prototypes -----------------------------------------------*/
+/* Function implementations --------------------------------------------------*/
+
+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();
+    }
+
+    static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) {
+        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<Ts...>::load_config(offset + size, crc16, vals...))
+            return -1;
+        return 0;
+    }
+
+    static int store_config(size_t offset, uint16_t* crc16, T* val0, 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<Ts...>::store_config(offset + size, crc16, vals...))
+            return -1;
+        return 0;
+    }
+
+    static int 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 = CRC16_INIT;
+        if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
+            return -1;
+        if (crc16)
+            return -1;
+        return 0;
+    }
+
+    static int store_config(T* val0, 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 = CRC16_INIT;
+        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;
+    }
+};
+
+// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
+// TODO: make this go away as part of the C++ refactoring
+void set_motor_config(MotorConfig_t *config, Motor_t *motor) {
+    motor->control_mode = config->control_mode;
+    motor->enable_step_dir = config->enable_step_dir;
+    motor->counts_per_step = config->counts_per_step;
+    motor->pos_setpoint = config->pos_setpoint;
+    motor->pos_gain = config->pos_gain;
+    motor->vel_setpoint = config->vel_setpoint;
+    motor->vel_gain = config->vel_gain;
+    motor->vel_integrator_gain = config->vel_integrator_gain;
+    motor->vel_integrator_current = config->vel_integrator_current;
+    motor->vel_limit = config->vel_limit;
+    motor->current_setpoint = config->current_setpoint;
+    motor->calibration_current = config->calibration_current;
+    motor->resistance_calib_max_voltage = config->resistance_calib_max_voltage;
+    motor->phase_inductance = config->phase_inductance;
+    motor->phase_resistance = config->phase_resistance;
+    motor->motor_type = config->motor_type;
+    motor->shunt_conductance = config->shunt_conductance;
+    motor->phase_current_rev_gain = config->phase_current_rev_gain;
+    motor->current_control = config->current_control;
+    motor->rotor_mode = config->rotor_mode;
+}
+
+// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
+// TODO: make this go away as part of the C++ refactoring
+void get_motor_config(Motor_t *motor, MotorConfig_t *config) {
+    config->control_mode = motor->control_mode;
+    config->enable_step_dir = motor->enable_step_dir;
+    config->counts_per_step = motor->counts_per_step;
+    config->pos_setpoint = motor->pos_setpoint;
+    config->pos_gain = motor->pos_gain;
+    config->vel_setpoint = motor->vel_setpoint;
+    config->vel_gain = motor->vel_gain;
+    config->vel_integrator_gain = motor->vel_integrator_gain;
+    config->vel_integrator_current = motor->vel_integrator_current;
+    config->vel_limit = motor->vel_limit;
+    config->current_setpoint = motor->current_setpoint;
+    config->calibration_current = motor->calibration_current;
+    config->resistance_calib_max_voltage = motor->resistance_calib_max_voltage;
+    config->phase_inductance = motor->phase_inductance;
+    config->phase_resistance = motor->phase_resistance;
+    config->motor_type = motor->motor_type;
+    config->shunt_conductance = motor->shunt_conductance;
+    config->phase_current_rev_gain = motor->phase_current_rev_gain;
+    config->current_control = motor->current_control;
+    config->rotor_mode = motor->rotor_mode;
+}
+
+
+void init_configuration(void) {
+    MotorConfig_t motor_config[2];
+    if (NVM_init() || Config<MotorConfig_t, MotorConfig_t>::load_config(&motor_config[0], &motor_config[1])) {
+        //printf("no config found\r\n"); osDelay(5);
+        // load default config
+        motor_config[0] = MotorConfig_t();
+        motor_config[1] = MotorConfig_t();
+    } else {
+        //printf("load config successful\r\n"); osDelay(5);
+    }
+    
+    set_motor_config(&motor_config[0], &motors[0]);
+    set_motor_config(&motor_config[1], &motors[1]);
+}
+
+void save_configuration(void) {
+    MotorConfig_t motor_config[2];
+    get_motor_config(&motors[0], &motor_config[0]);
+    get_motor_config(&motors[1], &motor_config[1]);
+    if (Config<MotorConfig_t, MotorConfig_t>::store_config(&motor_config[0], &motor_config[1])) {
+        //printf("saving configuration failed\r\n"); osDelay(5);
+    }
+}
+
+void reset(void) {
+    NVM_erase();
+    NVIC_SystemReset();
+}

Firmware/MotorControl/config.h

@@ -0,0 +1,16 @@
+#ifndef __CONFIG_H
+#define __CONFIG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void init_configuration(void);
+void save_configuration(void);
+void reset(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CONFIG_H */

Firmware/MotorControl/low_level.c

@@ -38,13 +38,6 @@ float vbus_voltage = 12.0f;
 #define POLE_PAIRS 7 // This value is correct for N5065 motors and Turnigy SK3 series.
 const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CPR);
 
-#if HW_VERSION_MAJOR == 3
-#if HW_VERSION_MINOR <= 3
-#define SHUNT_RESISTANCE (675e-6f)
-#else
-#define SHUNT_RESISTANCE (500e-6f)
-#endif
-#endif
 
 // TODO: Migrate to C++, clearly we are actually doing object oriented code here...
 // TODO: For nice encapsulation, consider not having the motor objects public
@@ -55,25 +48,7 @@ const float elec_rad_per_enc = POLE_PAIRS * 2 * M_PI * (1.0f / (float)ENCODER_CP
 Motor_t motors[] = {
     {
         // M0
-        .control_mode = CTRL_MODE_POSITION_CONTROL,  //see: Motor_control_mode_t
-        .enable_step_dir = false,                    //auto enabled after calibration
-        .counts_per_step = 2.0f,
         .error = ERROR_NO_ERROR,
-        .pos_setpoint = 0.0f,
-        .pos_gain = 20.0f,  // [(counts/s) / counts]
-        .vel_setpoint = 0.0f,
-        // .vel_setpoint = 800.0f, <sensorless example>
-        .vel_gain = 5.0f / 10000.0f,  // [A/(counts/s)]
-        // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
-        .vel_integrator_gain = 10.0f / 10000.0f,  // [A/(counts/s * s)]
-        // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] <sensorless example>
-        .vel_integrator_current = 0.0f,  // [A]
-        .vel_limit = 20000.0f,           // [counts/s]
-        .current_setpoint = 0.0f,        // [A]
-        .calibration_current = 10.0f,    // [A]
-        .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
-        .phase_inductance = 0.0f,        // to be set by measure_phase_inductance
-        .phase_resistance = 0.0f,        // to be set by measure_phase_resistance
         .motor_thread = 0,
         .thread_ready = false,
         // .enable_control = true,
@@ -96,29 +71,6 @@ Motor_t motors[] = {
             .enableTimeOut = false,
         },
         // .gate_driver_regs Init by DRV8301_setup
-        .motor_type = MOTOR_TYPE_HIGH_CURRENT,
-        // .motor_type = MOTOR_TYPE_GIMBAL,
-        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
-        .phase_current_rev_gain = 0.0f,                // to be set by DRV8301_setup
-        .current_control = {
-            // Read out max_allowed_current to see max supported value for current_lim.
-            // You can change DRV8301_ShuntAmpGain to get a different range.
-            // .current_lim = 75.0f, //[A]
-            .current_lim = 10.0f,  //[A]
-            .p_gain = 0.0f,        // [V/A] should be auto set after resistance and inductance measurement
-            .i_gain = 0.0f,        // [V/As] should be auto set after resistance and inductance measurement
-            .v_current_control_integral_d = 0.0f,
-            .v_current_control_integral_q = 0.0f,
-            .Ibus = 0.0f,
-            .final_v_alpha = 0.0f,
-            .final_v_beta = 0.0f,
-            .Iq_setpoint = 0.0f,
-            .Iq_measured = 0.0f,
-            .max_allowed_current = 0.0f,
-        },
-        // .rotor_mode = ROTOR_MODE_SENSORLESS,
-        // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS,
-        .rotor_mode = ROTOR_MODE_ENCODER,
         .encoder = {
             .encoder_timer = &htim3,
             .use_index = false,
@@ -163,22 +115,7 @@ Motor_t motors[] = {
         },
     },
     {                                             // M1
-        .control_mode = CTRL_MODE_POSITION_CONTROL,  //see: Motor_control_mode_t
-        .enable_step_dir = false,                    //auto enabled after calibration
-        .counts_per_step = 2.0f,
         .error = ERROR_NO_ERROR,
-        .pos_setpoint = 0.0f,
-        .pos_gain = 20.0f,  // [(counts/s) / counts]
-        .vel_setpoint = 0.0f,
-        .vel_gain = 5.0f / 10000.0f,             // [A/(counts/s)]
-        .vel_integrator_gain = 10.0f / 10000.0f,  // [A/(counts/s * s)]
-        .vel_integrator_current = 0.0f,           // [A]
-        .vel_limit = 20000.0f,                    // [counts/s]
-        .current_setpoint = 0.0f,                 // [A]
-        .calibration_current = 10.0f,             // [A]
-        .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
-        .phase_inductance = 0.0f,                 // to be set by measure_phase_inductance
-        .phase_resistance = 0.0f,                 // to be set by measure_phase_resistance
         .motor_thread = 0,
         .thread_ready = false,
         // .enable_control = true,
@@ -201,26 +138,6 @@ Motor_t motors[] = {
             .enableTimeOut = false,
         },
         // .gate_driver_regs Init by DRV8301_setup
-        .motor_type = MOTOR_TYPE_HIGH_CURRENT,
-        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
-        .phase_current_rev_gain = 0.0f,                // to be set by DRV8301_setup
-        .current_control = {
-            // Read out max_allowed_current to see max supported value for current_lim.
-            // You can change DRV8301_ShuntAmpGain to get a different range.
-            // .current_lim = 75.0f, //[A]
-            .current_lim = 10.0f,  //[A]
-            .p_gain = 0.0f,        // [V/A] should be auto set after resistance and inductance measurement
-            .i_gain = 0.0f,        // [V/As] should be auto set after resistance and inductance measurement
-            .v_current_control_integral_d = 0.0f,
-            .v_current_control_integral_q = 0.0f,
-            .Ibus = 0.0f,
-            .final_v_alpha = 0.0f,
-            .final_v_beta = 0.0f,
-            .Iq_setpoint = 0.0f,
-            .Iq_measured = 0.0f,
-            .max_allowed_current = 0.0f,
-        },
-        .rotor_mode = ROTOR_MODE_ENCODER,
         .encoder = {
             .encoder_timer = &htim4,
             .use_index = false,

Firmware/MotorControl/nvm.c

@@ -301,6 +301,8 @@ int NVM_start_write(size_t length) {
 //
 // The operation fails if (offset + length) is larger than the length passed to NVM_start_write.
 // The most recent valid NVM data is not modified or invalidated until NVM_commit is called.
+// Warning: Writing different data to the same area multiple times during a single transaction
+// will cause data corruption.
 //
 // @param offset: The offset in bytes, 0 being the beginning of the staging block.
 // @param data: Pointer to the data that should be written

Firmware/STM32F405RGTx_FLASH.ld

@@ -174,7 +174,7 @@ SECTIONS
     . = ALIGN(8);
   } >RAM
 
-
+  
 
   /* Remove information from the standard libraries */
   /DISCARD/ :

Firmware/Src/freertos.c

@@ -56,6 +56,7 @@
 #include "low_level.h"
 #include "axis_c_interface.h"
 #include "commands.h"
+#include "config.h"
 /* USER CODE END Includes */
 
 /* Variables -----------------------------------------------------------------*/
@@ -141,6 +142,9 @@ void StartDefaultTask(void const * argument)
 
   /* USER CODE BEGIN StartDefaultTask */
 
+  // Init and load persistent configuration
+  init_configuration();
+
   // Init communications
   init_communication();