mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-21 23:53:35 +08:00
253 lines
10 KiB
C++
253 lines
10 KiB
C++
|
|
/* Includes ------------------------------------------------------------------*/
|
|
|
|
#include "config.h"
|
|
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <stm32f405xx.h>
|
|
|
|
#include "nvm.h"
|
|
#include "crc.hpp"
|
|
#include "low_level.h"
|
|
#include "axis.h"
|
|
|
|
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
|
|
// the config structs, make sure to increment this number:
|
|
uint16_t config_version = 0x0001;
|
|
|
|
/* Private defines -----------------------------------------------------------*/
|
|
#define CRC16_INIT 0xabcd
|
|
|
|
/* Private macros ------------------------------------------------------------*/
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
|
|
typedef struct {
|
|
Motor_control_mode_t control_mode;
|
|
float counts_per_step;
|
|
int32_t pole_pairs;
|
|
float pos_gain;
|
|
float vel_gain;
|
|
float vel_integrator_gain;
|
|
float vel_limit;
|
|
float calibration_current;
|
|
float resistance_calib_max_voltage;
|
|
float phase_inductance;
|
|
float phase_resistance;
|
|
Motor_type_t motor_type;
|
|
Rotor_mode_t rotor_mode;
|
|
float current_control_current_lim;
|
|
bool encoder_use_index;
|
|
bool encoder_manually_calibrated;
|
|
float encoder_idx_search_speed;
|
|
int32_t encoder_cpr;
|
|
int32_t encoder_offset;
|
|
int32_t encoder_motor_dir;
|
|
} MotorConfig_t;
|
|
|
|
/* Global constant data ------------------------------------------------------*/
|
|
/* Global variables ----------------------------------------------------------*/
|
|
/* Private constant data -----------------------------------------------------*/
|
|
/* 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 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) {
|
|
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;
|
|
}
|
|
|
|
// @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<Ts...>::store_config(offset + size, crc16, vals...))
|
|
return -1;
|
|
return 0;
|
|
}
|
|
|
|
// @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 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 ^ config_version;
|
|
if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
|
|
return -1;
|
|
if (crc16)
|
|
return -1;
|
|
return 0;
|
|
}
|
|
|
|
// @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 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 = 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;
|
|
}
|
|
};
|
|
|
|
// 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(const MotorConfig_t* config, Motor_t* motor) {
|
|
motor->control_mode = config->control_mode;
|
|
motor->counts_per_step = config->counts_per_step;
|
|
motor->pole_pairs = config->pole_pairs;
|
|
motor->pos_gain = config->pos_gain;
|
|
motor->vel_gain = config->vel_gain;
|
|
motor->vel_integrator_gain = config->vel_integrator_gain;
|
|
motor->vel_limit = config->vel_limit;
|
|
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->rotor_mode = config->rotor_mode;
|
|
|
|
motor->current_control.current_lim = config->current_control_current_lim;
|
|
|
|
motor->encoder.use_index = config->encoder_use_index;
|
|
motor->encoder.manually_calibrated = config->encoder_manually_calibrated;
|
|
motor->encoder.idx_search_speed = config->encoder_idx_search_speed;
|
|
motor->encoder.encoder_cpr = config->encoder_cpr;
|
|
motor->encoder.encoder_offset = config->encoder_offset;
|
|
motor->encoder.motor_dir = config->encoder_motor_dir;
|
|
}
|
|
|
|
// 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(const Motor_t* motor, MotorConfig_t* config) {
|
|
config->control_mode = motor->control_mode;
|
|
config->counts_per_step = motor->counts_per_step;
|
|
config->pole_pairs = motor->pole_pairs;
|
|
config->pos_gain = motor->pos_gain;
|
|
config->vel_gain = motor->vel_gain;
|
|
config->vel_integrator_gain = motor->vel_integrator_gain;
|
|
config->vel_limit = motor->vel_limit;
|
|
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->rotor_mode = motor->rotor_mode;
|
|
|
|
config->current_control_current_lim = motor->current_control.current_lim;
|
|
|
|
config->encoder_use_index = motor->encoder.use_index;
|
|
config->encoder_manually_calibrated = motor->encoder.manually_calibrated;
|
|
config->encoder_idx_search_speed = motor->encoder.idx_search_speed;
|
|
config->encoder_cpr = motor->encoder.encoder_cpr;
|
|
config->encoder_offset = motor->encoder.encoder_offset;
|
|
config->encoder_motor_dir = motor->encoder.motor_dir;
|
|
}
|
|
|
|
|
|
void init_configuration(void) {
|
|
MotorConfig_t motor_config[2];
|
|
//TODO: we really shouldn't be hardcoding like this
|
|
if (NVM_init() || Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::load_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
|
|
//printf("no config found\r\n"); osDelay(5);
|
|
// load default config
|
|
// motor_config[0] = MotorConfig_t();
|
|
// motor_config[1] = MotorConfig_t();
|
|
|
|
// TODO: temporary hack, this is gonna change after refactoring
|
|
axis_configs[0] = AxisConfig();
|
|
axis_configs[1] = AxisConfig();
|
|
brake_resistance = 0.47f;
|
|
|
|
// Default config coming from flashed Motor_t
|
|
return;
|
|
} 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]);
|
|
//TODO: we really shouldn't be hardcoding like this
|
|
if (Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::store_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
|
|
//printf("saving configuration failed\r\n"); osDelay(5);
|
|
}
|
|
}
|
|
|
|
void erase_configuration(void) {
|
|
NVM_erase();
|
|
}
|