Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 08:04:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge branch 'devel' into sam_user_adc

Browse Source
This commit is contained in:
Oskar Weigl
2018-06-05 23:40:50 -07:00
parent 8339311c35 2a7743ef0b
commit f8306741bc
71 changed files with 16570 additions and 729 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+67 -66
View File
@@ -38,6 +38,7 @@ void Axis::setup() {
static void run_state_machine_loop_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
reinterpret_cast<Axis*>(ctx)->thread_id_valid_ = false;
}
// @brief Starts run_state_machine_loop in a new thread
@@ -92,16 +93,34 @@ void Axis::set_step_dir_enabled(bool enable) {
}
}
// @brief Returns true if everything is ok.
// Sets error and returns false otherwise.
// @brief Do axis level checks and call subcomponent do_checks
// Returns true if everything is ok.
bool Axis::do_checks() {
if (!motor_.do_checks())
return error_ |= ERROR_MOTOR_FAILED, false;
if (!brake_resistor_armed)
error_ |= ERROR_BRAKE_RESISTOR_DISARMED;
if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED))
// motor got disarmed in something other than the idle loop
error_ |= ERROR_MOTOR_DISARMED;
if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level))
return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false;
error_ |= ERROR_DC_BUS_UNDER_VOLTAGE;
if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level))
return error_ |= ERROR_DC_BUS_OVER_VOLTAGE, false;
return true;
error_ |= ERROR_DC_BUS_OVER_VOLTAGE;
// Sub-components should use set_error which will propegate to this error_
motor_.do_checks();
encoder_.do_checks();
// sensorless_estimator_.do_checks();
// controller_.do_checks();
return error_ == ERROR_NONE;
}
// @brief Update all esitmators
bool Axis::do_updates() {
// Sub-components should use set_error which will propegate to this error_
encoder_.update();
sensorless_estimator_.update();
return error_ == ERROR_NONE;
}
bool Axis::run_sensorless_spin_up() {
@@ -115,7 +134,7 @@ bool Axis::run_sensorless_spin_up() {
return error_ |= ERROR_MOTOR_FAILED, false;
return x < 1.0f;
});
if (error_ != ERROR_NO_ERROR)
if (error_ != ERROR_NONE)
return false;
// Late Spin-up: accelerate
@@ -129,49 +148,41 @@ bool Axis::run_sensorless_spin_up() {
return error_ |= ERROR_MOTOR_FAILED, false;
return vel < config_.spin_up_target_vel;
});
return error_ == ERROR_NO_ERROR;
return error_ == ERROR_NONE;
}
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
// We update the encoder just in case someone needs the output for testing
encoder_.update(nullptr, nullptr, nullptr);
if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase))
return error_ |= ERROR_SENSORLESS_ESTIMATOR_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.pll_vel_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
return false; // set_error should update axis.error_
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
return error_ == ERROR_NONE;
}
bool Axis::run_closed_loop_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
// We update the sensorless estimator just in case someone needs the output for testing
sensorless_estimator_.update(nullptr, nullptr, nullptr);
if (!encoder_.update(&pos_estimate, &vel_estimate, &phase))
return error_ |= ERROR_ENCODER_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(encoder_.pos_estimate_, encoder_.pll_vel_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
if (!motor_.update(current_setpoint, encoder_.phase_))
return false; // set_error should update axis.error_
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
return error_ == ERROR_NONE;
}
bool Axis::run_idle_loop() {
@@ -179,11 +190,9 @@ bool Axis::run_idle_loop() {
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
run_control_loop([this](){
sensorless_estimator_.update(nullptr, nullptr, nullptr);
encoder_.update(nullptr, nullptr, nullptr);
return true;
});
return error_ == ERROR_NO_ERROR;
return error_ == ERROR_NONE;
}
// Infinite loop that does calibration and enters main control loop as appropriate
@@ -246,43 +255,37 @@ void Axis::run_state_machine_loop() {
// Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
bool status;
switch (current_state_) {
case AXIS_STATE_MOTOR_CALIBRATION:
status = motor_.run_calibration();
if (!status)
error_ |= ERROR_MOTOR_FAILED;
break;
case AXIS_STATE_MOTOR_CALIBRATION:
status = motor_.run_calibration();
break;
case AXIS_STATE_ENCODER_INDEX_SEARCH:
status = encoder_.run_index_search();
if (!status)
error_ |= ERROR_ENCODER_FAILED;
break;
case AXIS_STATE_ENCODER_INDEX_SEARCH:
status = encoder_.run_index_search();
break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
status = encoder_.run_offset_calibration();
if (!status)
error_ |= ERROR_ENCODER_FAILED;
break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
status = encoder_.run_offset_calibration();
break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
status = run_sensorless_control_loop();
break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
status = run_sensorless_control_loop();
break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
status = run_closed_loop_control_loop();
break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
status = run_closed_loop_control_loop();
break;
case AXIS_STATE_IDLE:
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
break;
case AXIS_STATE_IDLE:
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
break;
default:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
default:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
}
// If the state failed, go to idle, else advance task chain
@@ -291,6 +294,4 @@ void Axis::run_state_machine_loop() {
else
memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
}
thread_id_valid_ = false;
}
Firmware/MotorControl/axis.hpp
+5 -16
View File
@@ -42,7 +42,7 @@ struct AxisConfig_t {
class Axis {
public:
enum Error_t {
ERROR_NO_ERROR = 0x00,
ERROR_NONE = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
@@ -78,6 +78,7 @@ public:
bool check_DRV_fault();
bool check_PSU_brownout();
bool do_checks();
bool do_updates();
// @brief Runs the specified update handler at the frequency of the current measurements.
//
@@ -102,21 +103,9 @@ public:
template<typename T>
void run_control_loop(const T& update_handler) {
while (requested_state_ == AXIS_STATE_UNDEFINED) {
if (!brake_resistor_armed_) {
error_ |= ERROR_BRAKE_RESISTOR_DISARMED;
if (!do_checks()) // look for errors at axis level and also all subcomponents
break;
}
if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) {
// motor got disarmed in something other than the idle loop
error_ |= ERROR_MOTOR_DISARMED;
break;
}
if (motor_.error_ != Motor::ERROR_NO_ERROR) {
error_ |= ERROR_MOTOR_FAILED;
break;
}
if (!do_checks()) // error set during function call
if (!do_updates()) // Update all estimators
break;
// Run main loop function, defer quitting for after wait
@@ -160,7 +149,7 @@ public:
volatile bool thread_id_valid_ = false;
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
Error_t error_ = ERROR_NONE;
bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
Firmware/MotorControl/board_config_v3.h
+27
View File
@@ -32,6 +32,12 @@ typedef struct {
TIM_HandleTypeDef* timer;
GPIO_TypeDef* index_port;
uint16_t index_pin;
GPIO_TypeDef* hallA_port;
uint16_t hallA_pin;
GPIO_TypeDef* hallB_port;
uint16_t hallB_pin;
GPIO_TypeDef* hallC_port;
uint16_t hallC_pin;
} EncoderHardwareConfig_t;
typedef struct {
TIM_HandleTypeDef* timer;
@@ -56,6 +62,7 @@ typedef struct {
extern const BoardHardwareConfig_t hw_configs[2];
//TODO stick this in a C file
#ifdef __MAIN_CPP__
const BoardHardwareConfig_t hw_configs[2] = { {
.axis_config = {
@@ -69,6 +76,12 @@ const BoardHardwareConfig_t hw_configs[2] = { {
.timer = &htim3,
.index_port = M0_ENC_Z_GPIO_Port,
.index_pin = M0_ENC_Z_Pin,
.hallA_port = M0_ENC_A_GPIO_Port,
.hallA_pin = M0_ENC_A_Pin,
.hallB_port = M0_ENC_B_GPIO_Port,
.hallB_pin = M0_ENC_B_Pin,
.hallC_port = M0_ENC_Z_GPIO_Port,
.hallC_pin = M0_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim1,
@@ -97,6 +110,12 @@ const BoardHardwareConfig_t hw_configs[2] = { {
.timer = &htim4,
.index_port = M1_ENC_Z_GPIO_Port,
.index_pin = M1_ENC_Z_Pin,
.hallA_port = M1_ENC_A_GPIO_Port,
.hallA_pin = M1_ENC_A_Pin,
.hallB_port = M1_ENC_B_GPIO_Port,
.hallB_pin = M1_ENC_B_Pin,
.hallC_port = M1_ENC_Z_GPIO_Port,
.hallC_pin = M1_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim8,
@@ -117,4 +136,12 @@ const BoardHardwareConfig_t hw_configs[2] = { {
#endif
#define I2C_A0_PORT GPIO_3_GPIO_Port
#define I2C_A0_PIN GPIO_3_Pin
#define I2C_A1_PORT GPIO_4_GPIO_Port
#define I2C_A1_PIN GPIO_4_Pin
#define I2C_A2_PORT GPIO_5_GPIO_Port
#define I2C_A2_PIN GPIO_5_Pin
#endif // __BOARD_CONFIG_H
Firmware/MotorControl/controller.cpp
+1 -1
View File
@@ -46,7 +46,7 @@ void Controller::set_current_setpoint(float current_setpoint) {
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) {
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) {
anticogging_.calib_anticogging = true;
}
}
Firmware/MotorControl/controller.hpp
+1 -1
View File
@@ -18,7 +18,7 @@ struct ControllerConfig_t {
Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
// float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s]
};
Firmware/MotorControl/encoder.cpp
+106 -35
View File
@@ -3,7 +3,7 @@
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config) :
Config_t& config) :
hw_config_(hw_config),
config_(config)
{
@@ -14,6 +14,11 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
// Critically damped
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) {
offset_ = config.offset;
is_ready_ = true;
}
}
static void enc_index_cb_wrapper(void* ctx) {
@@ -26,6 +31,15 @@ void Encoder::setup() {
enc_index_cb_wrapper, this);
}
void Encoder::set_error(Encoder::Error_t error) {
error_ |= error;
axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
}
bool Encoder::do_checks(){
return error_ == ERROR_NONE;
}
//--------------------
// Hardware Dependent
//--------------------
@@ -105,7 +119,7 @@ bool Encoder::run_index_search() {
// continue until the index is found
return !index_found_;
});
return axis_->error_ != Axis::ERROR_NO_ERROR;
return true;
}
// @brief Turns the motor in one direction for a bit and then in the other
@@ -139,10 +153,10 @@ bool Encoder::run_offset_calibration() {
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
if (axis_->error_ != Axis::ERROR_NONE)
return false;
int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT;
int32_t init_enc_val = shadow_count_;
int64_t encvaluesum = 0;
// scan forward
@@ -155,32 +169,32 @@ bool Encoder::run_offset_calibration() {
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
if (axis_->error_ != Axis::ERROR_NONE)
return false;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = scan_distance / elec_rad_per_enc;
float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val);
float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
error_ |= ERROR_CPR_OUT_OF_RANGE;
set_error(ERROR_CPR_OUT_OF_RANGE);
return false;
}
// check direction
if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) {
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
} else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) {
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
} else {
// Encoder response error
error_ |= ERROR_RESPONSE;
set_error(ERROR_RESPONSE);
return false;
}
@@ -194,61 +208,118 @@ bool Encoder::run_offset_calibration() {
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
if (axis_->error_ != Axis::ERROR_NONE)
return false;
offset_ = encvaluesum / (num_steps * 2);
config_.offset = offset_;
int32_t residual = encvaluesum - ((int64_t)offset_ * (int64_t)(num_steps * 2));
config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
config_.use_index = old_use_index;
return true;
}
bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
switch (hall_state) {
case 0b001: *hall_cnt = 0; return true;
case 0b011: *hall_cnt = 1; return true;
case 0b010: *hall_cnt = 2; return true;
case 0b110: *hall_cnt = 3; return true;
case 0b100: *hall_cnt = 4; return true;
case 0b101: *hall_cnt = 5; return true;
default: return false;
}
}
bool Encoder::update() {
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ |= ERROR_NUMERICAL;
set_error(ERROR_UNSTABLE_GAIN);
return false;
}
// update internal encoder state
int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
int32_t delta_enc = (int32_t)delta_enc_16; //sign extend
// update internal encoder state.
int32_t delta_enc = 0;
switch (config_.mode) {
case MODE_INCREMENTAL: {
//TODO: use count_in_cpr_ instead as shadow_count_ can overflow
//or use 64 bit
int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
delta_enc = (int32_t)delta_enc_16; //sign extend
} break;
case MODE_HALL: {
int32_t hall_cnt;
if (decode_hall(hall_state_, &hall_cnt)) {
delta_enc = hall_cnt - count_in_cpr_;
delta_enc = mod(delta_enc, 6);
if (delta_enc > 3)
delta_enc -= 6;
} else {
set_error(ERROR_ILLEGAL_HALL_STATE);
return false;
}
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return false;
} break;
}
shadow_count_ += delta_enc;
count_in_cpr_ += delta_enc;
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
// compute electrical phase
int corrected_enc = count_in_cpr_ - offset_;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (float)corrected_enc;
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
// run pll (for now pll is in units of encoder counts)
//// run pll (for now pll is in units of encoder counts)
// Predict current pos
pos_estimate_ += current_meas_period * pll_vel_;
pos_cpr_ += current_meas_period * pll_vel_;
pos_cpr_ += current_meas_period * pll_vel_;
// discrete phase detector
float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr;
if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_)
bool snap_to_zero_vel = false;
if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_) {
pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter
snap_to_zero_vel = true;
}
//// run encoder count interpolation
int32_t corrected_enc = count_in_cpr_ - offset_;
// if we are stopped, make sure we don't randomly drift
if (snap_to_zero_vel) {
interpolation_ = 0.5f;
// reset interpolation if encoder edge comes
} else if (delta_enc > 0) {
interpolation_ = 0.0f;
} else if (delta_enc < 0) {
interpolation_ = 1.0f;
} else {
// Interpolate (predict) between encoder counts using pll_vel,
interpolation_ += current_meas_period * pll_vel_;
// don't allow interpolation indicated position outside of [enc, enc+1)
if (interpolation_ > 1.0f) interpolation_ = 1.0f;
if (interpolation_ < 0.0f) interpolation_ = 0.0f;
}
float interpolated_enc = corrected_enc + interpolation_;
//// compute electrical phase
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
// Assign output arguments
if (pos_estimate) *pos_estimate = pos_estimate_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
}
Firmware/MotorControl/encoder.hpp
+37 -18
View File
@@ -5,33 +5,44 @@
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
struct EncoderConfig_t {
bool use_index = false;
bool pre_calibrated = false; // If true, this means the offset stored in
// configuration is valid and does not need
// be determined by run_offset_calibration.
// In this case the encoder will enter ready
// state as soon as the index is found.
float idx_search_speed = 10.0f; // [rad/s electrical]
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once
// index search succeeds
float calib_range = 0.02f;
};
class Encoder {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_NUMERICAL = 0x01,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_OUT_OF_RANGE = 0x02,
ERROR_RESPONSE = 0x04,
ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
ERROR_ILLEGAL_HALL_STATE = 0x10,
};
enum Mode_t {
MODE_INCREMENTAL,
MODE_HALL
};
struct Config_t {
Encoder::Mode_t mode = Encoder::MODE_INCREMENTAL;
bool use_index = false;
bool pre_calibrated = false; // If true, this means the offset stored in
// configuration is valid and does not need
// be determined by run_offset_calibration.
// In this case the encoder will enter ready
// state as soon as the index is found.
float idx_search_speed = 10.0f; // [rad/s electrical]
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once
// index search succeeds
float offset_float = 0.0f; // Sub-count phase alignment offset
float calib_range = 0.02f;
};
Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config);
Config_t& config);
void setup();
void set_error(Error_t error);
bool do_checks();
void enc_index_cb();
@@ -42,10 +53,10 @@ public:
bool run_index_search();
bool run_offset_calibration();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
bool update();
const EncoderHardwareConfig_t& hw_config_;
EncoderConfig_t& config_;
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error_t error_ = ERROR_NONE;
@@ -54,6 +65,7 @@ public:
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
int32_t offset_ = 0;
float interpolation_ = 0.0f;
float phase_ = 0.0f; // [rad]
float pos_estimate_ = 0.0f; // [rad]
float pos_cpr_ = 0.0f; // [rad]
@@ -61,6 +73,9 @@ public:
float pll_kp_ = 0.0f; // [rad/s / rad]
float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
// Updated by low_level pwm_adc_cb
uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
@@ -70,18 +85,22 @@ public:
make_protocol_property("shadow_count", &shadow_count_),
make_protocol_property("count_in_cpr", &count_in_cpr_),
make_protocol_property("offset", &offset_),
make_protocol_property("interpolation", &interpolation_),
make_protocol_property("phase", &phase_),
make_protocol_property("pos_estimate", &pos_estimate_),
make_protocol_property("pos_cpr", &pos_cpr_),
make_protocol_property("hall_state", &hall_state_),
make_protocol_property("pll_vel", &pll_vel_),
make_protocol_property("pll_kp", &pll_kp_),
make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("mode", &config_.mode),
make_protocol_property("use_index", &config_.use_index),
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("idx_search_speed", &config_.idx_search_speed),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("offset_float", &config_.offset_float),
make_protocol_property("calib_range", &config_.calib_range)
)
);
Firmware/MotorControl/low_level.cpp
+74 -20
View File
@@ -29,14 +29,18 @@
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
bool brake_resistor_armed_ = false;
bool brake_resistor_armed = false;
/* Private constant data -----------------------------------------------------*/
static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC };
static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]);
/* Private variables ---------------------------------------------------------*/
// Two motors, sampling port A,B,C (coherent with current meas timing)
static uint16_t GPIO_port_samples [2][num_GPIO];
/* CPU critical section helpers ----------------------------------------------*/
static inline uint8_t cpu_enter_critical() {
@@ -94,13 +98,23 @@ static inline void cpu_exit_critical(uint8_t status_register) {
* at a high rate.
*/
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error_t error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
axes[i]->motor_.error_ |= error;
}
safety_critical_disarm_brake_resistor();
}
// @brief Kicks off the arming process of the motor.
// All calls to this function must clearly originate
// from user input.
void safety_critical_arm_motor_pwm(Motor& motor) {
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
if (brake_resistor_armed) {
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
}
cpu_exit_critical(sr);
@@ -127,7 +141,7 @@ bool safety_critical_disarm_motor_pwm(Motor& motor) {
// timer period.
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) {
uint8_t sr = cpu_enter_critical();
if (!brake_resistor_armed_) {
if (!brake_resistor_armed) {
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
}
@@ -158,7 +172,7 @@ void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3])
// @brief Arms the brake resistor
void safety_critical_arm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = true;
brake_resistor_armed = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
cpu_exit_critical(sr);
@@ -170,7 +184,7 @@ void safety_critical_arm_brake_resistor() {
// by calling safety_critical_arm_brake_resistor().
void safety_critical_disarm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = false;
brake_resistor_armed = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
@@ -183,9 +197,9 @@ void safety_critical_disarm_brake_resistor() {
// the brake resistor is disarmed.
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) {
if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS)
for(;;);
low_level_fault(Motor::ERROR_BRAKE_DEADTIME_VIOLATION);
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
if (brake_resistor_armed) {
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
@@ -225,6 +239,10 @@ void start_adc_pwm() {
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8);
// Enable the update interrupt (used to coherently sample GPIO)
__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
__HAL_TIM_ENABLE_IT(&htim8, TIM_IT_UPDATE);
// Start brake resistor PWM in floating output configuration
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
@@ -303,17 +321,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
htim_b->Instance->BDTR |= MOE_store_b;
}
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error_t error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
axes[i]->motor_.error_ |= error;
}
safety_critical_disarm_brake_resistor();
}
// @brief ADC1 measurements are written to this buffer by DMA
uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = { 0 };
@@ -424,7 +431,6 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
// IRQ Callbacks
//--------------------------------
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12);
// Only one conversion in sequence, so only rank1
@@ -437,6 +443,35 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
}
}
static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
GPIO_TypeDef* hall_ports[] = {
enc.hw_config_.hallC_port,
enc.hw_config_.hallB_port,
enc.hw_config_.hallA_port,
};
uint16_t hall_pins[] = {
enc.hw_config_.hallC_pin,
enc.hw_config_.hallB_pin,
enc.hw_config_.hallA_pin,
};
uint8_t hall_state = 0x0;
for (int i = 0; i < 3; ++i) {
int port_idx = 0;
for (;;) {
auto port = GPIOs_to_samp[port_idx];
if (port == hall_ports[i])
break;
++port_idx;
}
hall_state <<= 1;
hall_state |= (GPIO_samples[port_idx] & hall_pins[i]) ? 1 : 0;
}
enc.hall_state_ = hall_state;
}
// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
// TODO: Document how the phasing is done, link to timing diagram
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
@@ -454,6 +489,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
// If we are counting down, we just sampled in SVM vector 7, with zero current
Axis& axis = injected ? *axes[0] : *axes[1];
int axis_num = injected ? 0 : 1;
Axis& other_axis = injected ? *axes[1] : *axes[0];
bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
@@ -512,6 +548,8 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
} else {
axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
}
// Prepare hall readings
decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]);
// Trigger axis thread
axis.signal_current_meas();
} else {
@@ -524,6 +562,22 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
}
}
void tim_update_cb(TIM_HandleTypeDef* htim) {
int portsamples_arr;
if (htim == &htim1) {
portsamples_arr = 0;
} else if (htim == &htim8) {
portsamples_arr = 1;
} else {
low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK);
return;
}
for (int i = 0; i < num_GPIO; ++i) {
GPIO_port_samples[portsamples_arr][i] = GPIOs_to_samp[i]->IDR;
}
}
// @brief Sums up the Ibus contribution of each motor and updates the
// brake resistor PWM accordingly.
void update_brake_current() {
Firmware/MotorControl/low_level.h
+3
View File
@@ -18,6 +18,8 @@ extern "C" {
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern bool brake_resistor_armed;
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
@@ -32,6 +34,7 @@ void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t hig
extern "C" {
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void tim_update_cb(TIM_HandleTypeDef* htim);
}
// Initalisation
Firmware/MotorControl/main.cpp
+29 -3
View File
@@ -6,9 +6,10 @@
#include "freertos_vars.h"
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
#include <communication/interface_i2c.h>
BoardConfig_t board_config;
EncoderConfig_t encoder_configs[AXIS_COUNT];
Encoder::Config_t encoder_configs[AXIS_COUNT];
ControllerConfig_t controller_configs[AXIS_COUNT];
MotorConfig_t motor_configs[AXIS_COUNT];
AxisConfig_t axis_configs[AXIS_COUNT];
@@ -20,7 +21,7 @@ Axis *axes[AXIS_COUNT];
typedef Config<
BoardConfig_t,
EncoderConfig_t[AXIS_COUNT],
Encoder::Config_t[AXIS_COUNT],
ControllerConfig_t[AXIS_COUNT],
MotorConfig_t[AXIS_COUNT],
AxisConfig_t[AXIS_COUNT]> ConfigFormat;
@@ -33,6 +34,8 @@ void save_configuration(void) {
&motor_configs,
&axis_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
} else {
user_config_loaded_ = true;
}
}
@@ -48,7 +51,7 @@ void load_configuration(void) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = EncoderConfig_t();
encoder_configs[i] = Encoder::Config_t();
controller_configs[i] = ControllerConfig_t();
motor_configs[i] = MotorConfig_t();
axis_configs[i] = AxisConfig_t();
@@ -104,6 +107,29 @@ int odrive_main(void) {
// Load persistent configuration (or defaults)
load_configuration();
if (board_config.enable_i2c_instead_of_can) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Pin = I2C_A0_PIN;
HAL_GPIO_Init(I2C_A0_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = I2C_A1_PIN;
HAL_GPIO_Init(I2C_A1_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = I2C_A2_PIN;
HAL_GPIO_Init(I2C_A2_PORT, &GPIO_InitStruct);
osDelay(1);
i2c_stats_.addr = (0xD << 3);
i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A0_PORT, I2C_A0_PIN) != GPIO_PIN_RESET ? 0x1 : 0;
i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A1_PORT, I2C_A1_PIN) != GPIO_PIN_RESET ? 0x2 : 0;
i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A2_PORT, I2C_A2_PIN) != GPIO_PIN_RESET ? 0x4 : 0;
MX_I2C1_Init(i2c_stats_.addr);
} else {
MX_CAN1_Init();
}
// Init general user ADC on some GPIOs.
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
Firmware/MotorControl/motor.cpp
+51 -36
View File
@@ -65,41 +65,51 @@ void Motor::update_current_controller_gains() {
// @brief Set up the gate drivers
void Motor::DRV8301_setup() {
DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
DRV8301_enable(&gate_driver_);
DRV8301_setupSpi(&gate_driver_, local_regs);
// TODO we can use reporting only if we actually wire up the nOCTW pin
local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
// Overcurrent set to approximately 150A at 100degC. This may need tweaking.
local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V;
// for reference:
// 20V/V on 500uOhm gives a range of +/- 150A
// 40V/V on 500uOhm gives a range of +/- 75A
// 20V/V on 666uOhm gives a range of +/- 110A
// 40V/V on 666uOhm gives a range of +/- 55A
local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV;
// local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV;
switch (local_regs->Ctrl_Reg_2.GAIN) {
case DRV8301_ShuntAmpGain_10VpV:
phase_current_rev_gain_ = 1.0f / 10.0f;
break;
case DRV8301_ShuntAmpGain_20VpV:
phase_current_rev_gain_ = 1.0f / 20.0f;
break;
case DRV8301_ShuntAmpGain_40VpV:
phase_current_rev_gain_ = 1.0f / 40.0f;
break;
case DRV8301_ShuntAmpGain_80VpV:
phase_current_rev_gain_ = 1.0f / 80.0f;
break;
}
// Solve for exact gain, then snap down to have equal or larger range as requested
// or largest possible range otherwise
static const float kMargin = 0.90f;
static const float max_output_swing = 1.6f; // [V] out of amplifier
float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A]
float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]
float margin = 0.90f;
float max_input = margin * 0.3f * hw_config_.shunt_conductance;
float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_;
current_control_.max_allowed_current = std::min(max_input, max_swing);
// Decoding array for snapping gain
std::array<std::pair<float, DRV8301_ShuntAmpGain_e>, 4> gain_choices = {
std::make_pair(10.0f, DRV8301_ShuntAmpGain_10VpV),
std::make_pair(20.0f, DRV8301_ShuntAmpGain_20VpV),
std::make_pair(40.0f, DRV8301_ShuntAmpGain_40VpV),
std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV)
};
// We use lower_bound in reverse because it snaps up by default, we want to snap down.
auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain,
[](std::pair<float, DRV8301_ShuntAmpGain_e> pair, float val){
return pair.first > val;
});
// If we snap to outside the array, clip to smallest val
if(gain_snap_down == gain_choices.crend())
--gain_snap_down;
// Values for current controller
phase_current_rev_gain_ = 1.0f / gain_snap_down->first;
// Clip all current control to actual usable range
current_control_.max_allowed_current = max_unity_gain_current * phase_current_rev_gain_;
// We now have the gain settings we want to use, lets set up DRV chip
DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
DRV8301_enable(&gate_driver_);
DRV8301_setupSpi(&gate_driver_, local_regs);
local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
// Overcurrent set to approximately 150A at 100degC. This may need tweaking.
local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V;
local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second;
local_regs->SndCmd = true;
DRV8301_writeData(&gate_driver_, local_regs);
@@ -124,9 +134,14 @@ bool Motor::check_DRV_fault() {
return true;
}
void Motor::set_error(Motor::Error_t error){
error_ |= error;
axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
}
bool Motor::do_checks() {
if (!check_DRV_fault()) {
error_ |= ERROR_DRV_FAULT;
set_error(ERROR_DRV_FAULT);
return false;
}
return true;
@@ -169,7 +184,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
float Ialpha = -(current_meas_.phB + current_meas_.phC);
test_voltage += (kI * current_meas_period) * (test_current - Ialpha);
if (test_voltage > max_voltage || test_voltage < -max_voltage)
return error_ |= ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
return set_error(ERROR_PHASE_RESISTANCE_OUT_OF_RANGE), false;
// Test voltage along phase A
if (!enqueue_voltage_timings(test_voltage, 0.0f))
@@ -178,7 +193,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
return ++i < num_test_cycles;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
if (axis_->error_ != Axis::ERROR_NONE)
return false;
//// De-energize motor
@@ -207,7 +222,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
return ++t < (num_cycles << 1);
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
if (axis_->error_ != Axis::ERROR_NONE)
return false;
//// De-energize motor
@@ -223,7 +238,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
config_.phase_inductance = L;
// TODO arbitrary values set for now
if (L < 1e-6f || L > 500e-6f)
return error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false;
return set_error(ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE), false;
return true;
}
@@ -250,7 +265,7 @@ bool Motor::run_calibration() {
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
float tA, tB, tC;
if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
return error_ |= ERROR_NUMERICAL, false;
return set_error(ERROR_MODULATION_MAGNITUDE), false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
@@ -358,7 +373,7 @@ bool Motor::update(float current_setpoint, float phase) {
if(!FOC_voltage(0.0f, current_setpoint, phase))
return false;
} else {
error_ |= ERROR_NOT_IMPLEMENTED_MOTOR_TYPE;
set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
return false;
}
return true;
Firmware/MotorControl/motor.hpp
+19 -14
View File
@@ -37,7 +37,7 @@ typedef struct {
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
typedef struct {
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series.
int32_t pole_pairs = 7;
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
@@ -46,23 +46,26 @@ typedef struct {
Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// You can change DRV8301_ShuntAmpGain to get a different range.
// float current_lim = 75.0f; //[A]
// float current_lim = 70.0f; //[A]
float current_lim = 10.0f; //[A]
// Value used to compute shunt amplifier gains
float requested_current_range = 70.0f; // [A]
} MotorConfig_t;
class Motor {
public:
enum Error_t {
ERROR_NO_ERROR = 0,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02,
ERROR_ADC_FAILED = 0x04,
ERROR_DRV_FAULT = 0x08,
ERROR_CONTROL_DEADLINE_MISSED = 0x10,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x20,
ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x40,
ERROR_NUMERICAL = 0x80
ERROR_NONE = 0,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x0001,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x0002,
ERROR_ADC_FAILED = 0x0004,
ERROR_DRV_FAULT = 0x0008,
ERROR_CONTROL_DEADLINE_MISSED = 0x0010,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x0020,
ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x0040,
ERROR_MODULATION_MAGNITUDE = 0x0080,
ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100,
ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200
};
enum TimingLog_t {
@@ -100,6 +103,7 @@ public:
void update_current_controller_gains();
void DRV8301_setup();
bool check_DRV_fault();
void set_error(Error_t error);
bool do_checks();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
@@ -131,7 +135,7 @@ public:
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 };
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
Error_t error_ = ERROR_NONE;
// Do not write to this variable directly!
// It is for exclusive use by the safety_critical_... functions.
ArmedState_t armed_state_ = ARMED_STATE_DISARMED;
@@ -204,7 +208,8 @@ public:
make_protocol_property("phase_resistance", &config_.phase_resistance),
make_protocol_property("direction", &config_.direction),
make_protocol_property("motor_type", &config_.motor_type),
make_protocol_property("current_lim", &config_.current_lim)
make_protocol_property("current_lim", &config_.current_lim),
make_protocol_property("requested_current_range", &config_.requested_current_range)
)
);
}
Firmware/MotorControl/odrive_main.h
+6 -3
View File
@@ -7,6 +7,8 @@ extern "C" {
// STM specific includes
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#include <can.h>
#include <i2c.h>
#define ARM_MATH_CM4 // TODO: might change in future board versions
#include <arm_math.h>
@@ -23,11 +25,10 @@ extern "C" {
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
//TODO clean this up
static const float current_meas_period = CURRENT_MEAS_PERIOD;
static const int current_meas_hz = CURRENT_MEAS_HZ;
extern float vbus_voltage;
extern bool brake_resistor_armed_;
extern const float elec_rad_per_enc;
// extern const float elec_rad_per_enc;
extern uint32_t _reboot_cookie;
extern bool user_config_loaded_;
@@ -57,6 +58,8 @@ extern SystemStats_t system_stats_;
// @brief general user configurable board configuration
struct BoardConfig_t {
bool enable_uart = true;
bool enable_i2c_instead_of_can = false;
bool enable_ascii_protocol_on_usb = true;
float brake_resistance = 0.47f; // [ohm]
float dc_bus_undervoltage_trip_level = 8.0f; //<! [V] minimum voltage below which the motor stops operating
float dc_bus_overvoltage_trip_level = 1.08f * HW_VERSION_VOLTAGE; //<! [V] maximum voltage above which the motor stops operating.
Firmware/MotorControl/sensorless_estimator.cpp
+2 -18
View File
@@ -12,7 +12,7 @@ SensorlessEstimator::SensorlessEstimator()
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
}
bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
bool SensorlessEstimator::update() {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).
@@ -23,7 +23,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ |= ERROR_NUMERICAL;
error_ |= ERROR_UNSTABLE_GAIN;
return false;
}
@@ -83,21 +83,5 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float
// update PLL velocity
pll_vel_ += current_meas_period * pll_ki_ * delta_phase;
//TODO TEMP TEST HACK
// static int trigger_ctr = 0;
// if (++trigger_ctr >= 3*current_meas_hz) {
// trigger_ctr = 0;
// //Change to sensorless units
// motor->vel_gain = 15.0f / 200.0f;
// motor->vel_setpoint = 800.0f * motor->encoder.motor_dir;
// //Change mode
// motor->rotor_mode = ROTOR_MODE_SENSORLESS;
// }
if (pos_estimate) *pos_estimate = pll_pos_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
};
Firmware/MotorControl/sensorless_estimator.hpp
+2 -2
View File
@@ -5,12 +5,12 @@ class SensorlessEstimator {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_NUMERICAL = 0x01,
ERROR_UNSTABLE_GAIN = 0x01,
};
SensorlessEstimator();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
bool update();
Axis* axis_ = nullptr; // set by Axis constructor