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

Formatting pass

Browse Source
This commit is contained in:
Unknown
2019-08-20 23:13:54 -04:00
parent 51e81cc8f1
commit 2b58d15d8d
20 changed files with 877 additions and 917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+64 -69
View File
@@ -3,9 +3,9 @@
#include <functional>
#include "gpio.h"
#include "communication/interface_can.hpp"
#include "odrive_main.h"
#include "utils.h"
#include "communication/interface_can.hpp"
Axis::Axis(int axis_num,
const AxisHardwareConfig_t& hw_config,
@@ -26,13 +26,12 @@ Axis::Axis(int axis_num,
motor_(motor),
trap_(trap),
min_endstop_(min_endstop),
max_endstop_(max_endstop)
{
encoder_.axis_ = this;
max_endstop_(max_endstop) {
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
trap_.axis_ = this;
decode_step_dir_pins();
watchdog_feed();
min_endstop_.axis_ = this;
@@ -41,29 +40,29 @@ Axis::Axis(int axis_num,
Axis::LockinConfig_t Axis::default_calibration() {
Axis::LockinConfig_t config;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 20.0f; // [rad/s^2]
config.vel = 40.0f; // [rad/s]
config.finish_distance = 100.0f * 2.0f * M_PI; // [rad]
config.finish_on_vel = false;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 20.0f; // [rad/s^2]
config.vel = 40.0f; // [rad/s]
config.finish_distance = 100.0f * 2.0f * M_PI; // [rad]
config.finish_on_vel = false;
config.finish_on_distance = true;
config.finish_on_enc_idx = true;
config.finish_on_enc_idx = true;
return config;
}
Axis::LockinConfig_t Axis::default_sensorless() {
Axis::LockinConfig_t config;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 200.0f; // [rad/s^2]
config.vel = 400.0f; // [rad/s]
config.finish_distance = 100.0f; // [rad]
config.finish_on_vel = true;
config.current = 10.0f; // [A]
config.ramp_time = 0.4f; // [s]
config.ramp_distance = 1 * M_PI; // [rad]
config.accel = 200.0f; // [rad/s^2]
config.vel = 400.0f; // [rad/s]
config.finish_distance = 100.0f; // [rad]
config.finish_on_vel = true;
config.finish_on_distance = false;
config.finish_on_enc_idx = false;
config.finish_on_enc_idx = false;
return config;
}
@@ -71,7 +70,6 @@ static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
void Axis::setup() {
@@ -87,7 +85,7 @@ static void run_state_machine_loop_wrapper(void* ctx) {
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -108,27 +106,27 @@ bool Axis::wait_for_current_meas() {
void Axis::step_cb() {
if (step_dir_active_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(dir_port_, dir_pin_);
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.input_pos_ += dir * config_.counts_per_step;
controller_.input_pos_updated();
}
};
void Axis::load_default_step_dir_pin_config(
const AxisHardwareConfig_t& hw_config, Config_t* config) {
const AxisHardwareConfig_t& hw_config, Config_t* config) {
config->step_gpio_pin = hw_config.step_gpio_pin;
config->dir_gpio_pin = hw_config.dir_gpio_pin;
config->dir_gpio_pin = hw_config.dir_gpio_pin;
}
void Axis::load_default_can_id(const int& id, Config_t& config){
void Axis::load_default_can_id(const int& id, Config_t& config) {
config.can_node_id = id;
}
void Axis::decode_step_dir_pins() {
step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin);
dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin);
dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
}
// @brief (de)activates step/dir input
@@ -136,7 +134,7 @@ void Axis::set_step_dir_active(bool active) {
if (active) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Pin = dir_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
@@ -174,9 +172,8 @@ bool Axis::do_checks() {
}
}
if(board_config.power_supply_wattage > 0.0f &&
(Ibus_sum * vbus_voltage) > board_config.power_supply_wattage)
{
if (board_config.power_supply_wattage > 0.0f &&
(Ibus_sum * vbus_voltage) > board_config.power_supply_wattage) {
error_ |= ERROR_DC_BUS_OVER_POWER;
}
@@ -221,10 +218,10 @@ bool Axis::watchdog_check() {
}
}
bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
bool Axis::run_lockin_spin(const LockinConfig_t& lockin_config) {
// Spiral up current for softer rotor lock-in
lockin_state_ = LOCKIN_STATE_RAMP;
float x = 0.0f;
float x = 0.0f;
run_control_loop([&]() {
float phase = wrap_pm_pi(lockin_config.ramp_distance * x);
float I_mag = lockin_config.current * x;
@@ -233,11 +230,11 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
return false;
return x < 1.0f;
});
// Spin states
float distance = lockin_config.ramp_distance;
float phase = wrap_pm_pi(distance);
float vel = distance / lockin_config.ramp_time;
float phase = wrap_pm_pi(distance);
float vel = distance / lockin_config.ramp_time;
// Function of states to check if we are done
auto spin_done = [&](bool vel_override = false) -> bool {
@@ -260,7 +257,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
if (!motor_.update(lockin_config.current, phase, vel))
return false;
return !spin_done(true); //vel_override to go to next phase
return !spin_done(true); //vel_override to go to next phase
});
if (!encoder_.index_found_)
@@ -269,7 +266,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// Constant speed
if (!spin_done()) {
lockin_state_ = LOCKIN_STATE_CONST_VEL;
vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
run_control_loop([&]() {
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
@@ -286,7 +283,7 @@ bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// 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() {
run_control_loop([this](){
run_control_loop([this]() {
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
@@ -295,7 +292,7 @@ bool Axis::run_sensorless_control_loop() {
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
return false; // set_error should update axis.error_
return false; // set_error should update axis.error_
return true;
});
return check_for_errors();
@@ -305,14 +302,14 @@ bool Axis::run_closed_loop_control_loop() {
// To avoid any transient on startup, we intialize the setpoint to be the current position
controller_.pos_setpoint_ = encoder_.pos_estimate_;
set_step_dir_active(config_.enable_step_dir);
run_control_loop([this](){
run_control_loop([this]() {
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
float phase_vel = 2 * M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return false; // set_error should update axis.error_
// Handle the homing case
if (homing_.homing_state == HOMING_STATE_HOMING) {
@@ -325,21 +322,21 @@ bool Axis::run_closed_loop_control_loop() {
encoder_.set_linear_count(min_endstop_.config_.offset);
controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = 0.0f;
controller_.input_vel_ = 0.0f;
controller_.input_current_ = 0.0f;
homing_.homing_state = HOMING_STATE_MOVE_TO_ZERO;
}
} else if (homing_.homing_state == HOMING_STATE_MOVE_TO_ZERO) {
if(!min_endstop_.getEndstopState() && controller_.trajectory_done_){
if (!min_endstop_.getEndstopState() && controller_.trajectory_done_) {
controller_.config_.control_mode = homing_.storedControlMode;
controller_.config_.input_mode = homing_.storedInputMode;
homing_.homing_state = HOMING_STATE_IDLE;
homing_.isHomed = true;
controller_.config_.input_mode = homing_.storedInputMode;
homing_.homing_state = HOMING_STATE_IDLE;
homing_.isHomed = true;
}
} else {
// Check for endstop presses
@@ -367,7 +364,6 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
// arm!
motor_.arm();
@@ -382,12 +378,11 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_closed_loop_control){
if(config_.startup_homing)
if (config_.startup_closed_loop_control) {
if (config_.startup_homing)
task_chain_[pos++] = AXIS_STATE_HOMING;
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
}
else if (config_.startup_sensorless_control)
} else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
@@ -401,7 +396,7 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
}
@@ -419,7 +414,7 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction == 0)
goto invalid_state_label;
status = encoder_.run_index_search();
@@ -443,25 +438,25 @@ void Axis::run_state_machine_loop() {
} break;
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
status = run_lockin_spin(config_.lockin);
} break;
case AXIS_STATE_SENSORLESS_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
if (status) {
// call to controller.reset() that happend when arming means that vel_setpoint
// is zeroed. So we make the setpoint the spinup target for smooth transition.
controller_.vel_setpoint_ = config_.sensorless_ramp.vel;
status = run_sensorless_control_loop();
status = run_sensorless_control_loop();
}
} break;
case AXIS_STATE_CLOSED_LOOP_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
if (!motor_.is_calibrated_ || motor_.config_.direction == 0)
goto invalid_state_label;
if (!encoder_.is_ready_)
goto invalid_state_label;
@@ -470,7 +465,7 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_IDLE: {
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
status = motor_.arm(); // done with idling - try to arm the motor
} break;
default:
Firmware/MotorControl/axis.hpp
+115 -122
View File
@@ -5,7 +5,6 @@
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
enum HomingState_t {
HOMING_STATE_IDLE,
HOMING_STATE_HOMING,
@@ -13,52 +12,52 @@ enum HomingState_t {
};
class Axis {
public:
public:
enum Error_t {
ERROR_NONE = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
ERROR_NONE = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
ERROR_MOTOR_FAILED = 0x40, // Go to motor.hpp for information, check odrvX.axisX.motor.error for error value
ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
ERROR_MOTOR_FAILED = 0x40, // Go to motor.hpp for information, check odrvX.axisX.motor.error for error value
ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
ERROR_CONTROLLER_FAILED = 0x200,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
ERROR_MIN_ENDSTOP_PRESSED = 0x1000,
ERROR_MAX_ENDSTOP_PRESSED = 0x2000,
ERROR_ESTOP_REQUESTED = 0x4000,
ERROR_DC_BUS_OVER_POWER = 0x8000,
ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
ERROR_CONTROLLER_FAILED = 0x200,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
ERROR_MIN_ENDSTOP_PRESSED = 0x1000,
ERROR_MAX_ENDSTOP_PRESSED = 0x2000,
ERROR_ESTOP_REQUESTED = 0x4000,
ERROR_DC_BUS_OVER_POWER = 0x8000,
};
enum State_t {
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //<! run closed loop control
AXIS_STATE_LOCKIN_SPIN = 9, //<! run lockin spin
AXIS_STATE_ENCODER_DIR_FIND = 10,
AXIS_STATE_HOMING = 11, //<! run axis homing function
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
AXIS_STATE_CLOSED_LOOP_CONTROL = 8, //<! run closed loop control
AXIS_STATE_LOCKIN_SPIN = 9, //<! run lockin spin
AXIS_STATE_ENCODER_DIR_FIND = 10,
AXIS_STATE_HOMING = 11, //<! run axis homing function
};
struct LockinConfig_t {
float current = 10.0f; // [A]
float ramp_time = 0.4f; // [s]
float ramp_distance = 1 * M_PI; // [rad]
float accel = 20.0f; // [rad/s^2]
float vel = 40.0f; // [rad/s]
float finish_distance = 100.0f; // [rad]
bool finish_on_vel = false;
float current = 10.0f; // [A]
float ramp_time = 0.4f; // [s]
float ramp_distance = 1 * M_PI; // [rad]
float accel = 20.0f; // [rad/s^2]
float vel = 40.0f; // [rad/s]
float finish_distance = 100.0f; // [rad]
bool finish_on_vel = false;
bool finish_on_distance = false;
bool finish_on_enc_idx = false;
bool finish_on_enc_idx = false;
};
static LockinConfig_t default_calibration();
@@ -66,35 +65,35 @@ public:
static LockinConfig_t default_lockin();
struct Config_t {
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
// this only has an effect if encoder.config.use_index is also true
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool startup_homing = false; //<! enable homing after calibration/startup
bool enable_step_dir = false; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
// this only has an effect if encoder.config.use_index is also true
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool startup_homing = false; //<! enable homing after calibration/startup
bool enable_step_dir = false; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
float counts_per_step = 2.0f;
float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
// Defaults loaded from hw_config in load_configuration in main.cpp
uint16_t step_gpio_pin = 0;
uint16_t dir_gpio_pin = 0;
uint16_t dir_gpio_pin = 0;
LockinConfig_t calibration_lockin = default_calibration();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t sensorless_ramp = default_sensorless();
LockinConfig_t lockin;
uint8_t can_node_id = 0; // Both axes will have the same id to start
uint8_t can_node_id = 0; // Both axes will have the same id to start
uint32_t can_heartbeat_rate_ms = 100;
};
struct Homing_t {
HomingState_t homing_state = HOMING_STATE_IDLE;
HomingState_t homing_state = HOMING_STATE_IDLE;
Controller::ControlMode_t storedControlMode = Controller::CTRL_MODE_POSITION_CONTROL;
Controller::InputMode_t storedInputMode = Controller::INPUT_MODE_PASSTHROUGH;
bool isHomed = false;
Controller::InputMode_t storedInputMode = Controller::INPUT_MODE_PASSTHROUGH;
bool isHomed = false;
};
enum thread_signals {
@@ -109,15 +108,15 @@ public:
};
Axis(int axis_num,
const AxisHardwareConfig_t& hw_config,
Config_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor,
TrapezoidalTrajectory& trap,
Endstop& min_endstop,
Endstop& max_endstop);
const AxisHardwareConfig_t& hw_config,
Config_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor,
TrapezoidalTrajectory& trap,
Endstop& min_endstop,
Endstop& max_endstop);
void setup();
void start_thread();
@@ -175,18 +174,18 @@ public:
// go to zero.
//
// @tparam T Must be a callable type that takes no arguments and returns a bool
template<typename T>
template <typename T>
void run_control_loop(const T& update_handler) {
while (requested_state_ == AXIS_STATE_UNDEFINED) {
// look for errors at axis level and also all subcomponents
bool checks_ok = do_checks();
// Update all estimators
// Note: updates run even if checks fail
bool updates_ok = do_updates();
bool updates_ok = do_updates();
// make sure the watchdog is being fed.
// make sure the watchdog is being fed.
bool watchdog_ok = watchdog_check();
if (!checks_ok || !updates_ok || !watchdog_ok) {
// It's not useful to quit idle since that is the safe action
// Also leaving idle would rearm the motors
@@ -216,7 +215,7 @@ public:
}
}
bool run_lockin_spin(const LockinConfig_t &lockin_config);
bool run_lockin_spin(const LockinConfig_t& lockin_config);
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_idle_loop();
@@ -243,8 +242,8 @@ public:
volatile bool thread_id_valid_ = false;
// variables exposed on protocol
Error_t error_ = ERROR_NONE;
bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir
Error_t error_ = ERROR_NONE;
bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir
// updated from config in constructor, and on protocol hook
GPIO_TypeDef* step_port_;
@@ -252,16 +251,16 @@ public:
GPIO_TypeDef* dir_port_;
uint16_t dir_pin_;
State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
State_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
State_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
State_t task_chain_[10] = {AXIS_STATE_UNDEFINED};
State_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
Homing_t homing_;
uint32_t last_heartbeat_ = 0;
// watchdog
uint32_t watchdog_current_value_= 0;
uint32_t watchdog_current_value_ = 0;
// Communication protocol definitions
auto make_protocol_definitions() {
@@ -275,51 +274,47 @@ public:
make_protocol_ro_property("homing_state", &homing_.homing_state),
make_protocol_property("is_homed", &homing_.isHomed),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("startup_homing", &config_.startup_homing),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("watchdog_timeout", &config_.watchdog_timeout),
make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_object("calibration_lockin",
make_protocol_property("current", &config_.calibration_lockin.current),
make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance),
make_protocol_property("accel", &config_.calibration_lockin.accel),
make_protocol_property("vel", &config_.calibration_lockin.vel)
),
make_protocol_object("sensorless_ramp",
make_protocol_property("current", &config_.sensorless_ramp.current),
make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time),
make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance),
make_protocol_property("accel", &config_.sensorless_ramp.accel),
make_protocol_property("vel", &config_.sensorless_ramp.vel),
make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance),
make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx)
),
make_protocol_object("general_lockin",
make_protocol_property("current", &config_.lockin.current),
make_protocol_property("ramp_time", &config_.lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
make_protocol_property("accel", &config_.lockin.accel),
make_protocol_property("vel", &config_.lockin.vel),
make_protocol_property("finish_distance", &config_.lockin.finish_distance),
make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
),
make_protocol_property("can_node_id", &config_.can_node_id),
make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)
),
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("startup_homing", &config_.startup_homing),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("watchdog_timeout", &config_.watchdog_timeout),
make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
[](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
make_protocol_object("calibration_lockin",
make_protocol_property("current", &config_.calibration_lockin.current),
make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance),
make_protocol_property("accel", &config_.calibration_lockin.accel),
make_protocol_property("vel", &config_.calibration_lockin.vel)),
make_protocol_object("sensorless_ramp",
make_protocol_property("current", &config_.sensorless_ramp.current),
make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time),
make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance),
make_protocol_property("accel", &config_.sensorless_ramp.accel),
make_protocol_property("vel", &config_.sensorless_ramp.vel),
make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance),
make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx)),
make_protocol_object("general_lockin",
make_protocol_property("current", &config_.lockin.current),
make_protocol_property("ramp_time", &config_.lockin.ramp_time),
make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
make_protocol_property("accel", &config_.lockin.accel),
make_protocol_property("vel", &config_.lockin.vel),
make_protocol_property("finish_distance", &config_.lockin.finish_distance),
make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)),
make_protocol_property("can_node_id", &config_.can_node_id),
make_protocol_property("can_heartbeat_rate_ms", &config_.can_heartbeat_rate_ms)),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions()),
@@ -328,12 +323,10 @@ public:
make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()),
make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()),
make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed),
make_protocol_function("clear_errors", *this, &Axis::clear_errors)
);
make_protocol_function("clear_errors", *this, &Axis::clear_errors));
}
};
DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t)
#endif /* __AXIS_HPP */
Firmware/MotorControl/controller.cpp
+46 -49
View File
@@ -3,17 +3,15 @@
#include <algorithm>
Controller::Controller(Config_t& config) :
config_(config)
{
Controller::Controller(Config_t& config) : config_(config) {
update_filter_gains();
}
void Controller::reset() {
pos_setpoint_ = 0.0f;
vel_setpoint_ = 0.0f;
pos_setpoint_ = 0.0f;
vel_setpoint_ = 0.0f;
vel_integrator_current_ = 0.0f;
current_setpoint_ = 0.0f;
current_setpoint_ = 0.0f;
}
void Controller::set_error(Error_t error) {
@@ -35,13 +33,13 @@ void Controller::move_to_pos(float goal_point) {
axis_->trap_.config_.accel_limit,
axis_->trap_.config_.decel_limit);
traj_start_loop_count_ = axis_->loop_counter_;
trajectory_done_ = false;
trajectory_done_ = false;
}
void Controller::move_incremental(float displacement, bool from_input_pos = true){
if(from_input_pos){
void Controller::move_incremental(float displacement, bool from_input_pos = true) {
if (from_input_pos) {
input_pos_ += displacement;
} else{
} else {
input_pos_ = pos_setpoint_ + displacement;
}
@@ -62,17 +60,17 @@ void Controller::start_anticogging_calibration() {
bool Controller::home_axis() {
if (axis_->min_endstop_.config_.enabled) {
axis_->homing_.storedControlMode = config_.control_mode;
axis_->homing_.storedInputMode = config_.input_mode;
axis_->homing_.storedInputMode = config_.input_mode;
config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
config_.input_mode = INPUT_MODE_VEL_RAMP;
config_.input_mode = INPUT_MODE_VEL_RAMP;
input_pos_ = 0.0f;
input_pos_updated();
input_vel_ = -config_.homing_speed;
input_vel_ = -config_.homing_speed;
input_current_ = 0.0f;
axis_->homing_.isHomed = false;
axis_->homing_.isHomed = false;
axis_->homing_.homing_state = HOMING_STATE_HOMING;
} else {
return false;
@@ -96,19 +94,19 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
}
if (config_.anticogging.index < 3600) {
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
input_vel_ = 0.0f;
input_current_ = 0.0f;
input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
input_vel_ = 0.0f;
input_current_ = 0.0f;
input_pos_updated();
return false;
} else {
config_.anticogging.index = 0;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
input_pos_ = 0.0f; // Send the motor home
input_vel_ = 0.0f;
input_current_ = 0.0f;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
input_pos_ = 0.0f; // Send the motor home
input_vel_ = 0.0f;
input_current_ = 0.0f;
input_pos_updated();
anticogging_valid_ = true;
anticogging_valid_ = true;
config_.anticogging.calib_anticogging = false;
return true;
}
@@ -117,8 +115,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate)
}
void Controller::update_filter_gains() {
input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time
input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time
input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
}
namespace {
@@ -140,32 +138,32 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
// do nothing
} break;
case INPUT_MODE_PASSTHROUGH: {
pos_setpoint_ = input_pos_;
vel_setpoint_ = input_vel_;
pos_setpoint_ = input_pos_;
vel_setpoint_ = input_vel_;
current_setpoint_ = input_current_;
} break;
case INPUT_MODE_VEL_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
float full_step = input_vel_ - vel_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
float full_step = input_vel_ - vel_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
vel_setpoint_ += step;
current_setpoint_ = step / current_meas_period * config_.inertia;
} break;
case INPUT_MODE_POS_FILTER: {
// 2nd order pos tracking filter
float delta_pos = input_pos_ - pos_setpoint_; // Pos error
float delta_vel = input_vel_ - vel_setpoint_; // Vel error
float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback
current_setpoint_ = accel * config_.inertia; // Accel
vel_setpoint_ += current_meas_period * accel; // delta vel
pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
float delta_pos = input_pos_ - pos_setpoint_; // Pos error
float delta_vel = input_vel_ - vel_setpoint_; // Vel error
float accel = input_filter_kp_ * delta_pos + input_filter_ki_ * delta_vel; // Feedback
current_setpoint_ = accel * config_.inertia; // Accel
vel_setpoint_ += current_meas_period * accel; // delta vel
pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
} break;
// case INPUT_MODE_MIX_CHANNELS: {
// // NOT YET IMPLEMENTED
// } break;
case INPUT_MODE_TRAP_TRAJ: {
if(input_pos_updated_){
if (input_pos_updated_) {
move_to_pos(input_pos_);
input_pos_updated_ = false;
}
@@ -178,29 +176,28 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
if (t > axis_->trap_.Tf_) {
// Drop into position control mode when done to avoid problems on loop counter delta overflow
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
pos_setpoint_ = input_pos_;
vel_setpoint_ = 0.0f;
current_setpoint_ = 0.0f;
trajectory_done_ = true;
pos_setpoint_ = input_pos_;
vel_setpoint_ = 0.0f;
current_setpoint_ = 0.0f;
trajectory_done_ = true;
} else {
TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t);
pos_setpoint_ = traj_step.Y;
vel_setpoint_ = traj_step.Yd;
current_setpoint_ = traj_step.Ydd * config_.inertia;
pos_setpoint_ = traj_step.Y;
vel_setpoint_ = traj_step.Yd;
current_setpoint_ = traj_step.Ydd * config_.inertia;
}
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
} break;
default: {
set_error(ERROR_INVALID_INPUT_MODE);
return false;
}
}
// Position control
// TODO Decide if we want to use encoder or pll position here
float gain_scheduling_multiplier = 1.0f;
float vel_des = vel_setpoint_;
float vel_des = vel_setpoint_;
if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
float pos_err;
if (config_.setpoints_in_cpr) {
@@ -216,7 +213,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
pos_err = pos_setpoint_ - pos_estimate;
}
vel_des += config_.pos_gain * pos_err;
// V-shaped gain shedule based on position error
// V-shaped gain shedule based on position error
float abs_pos_err = fabsf(pos_err);
if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) {
gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width;
@@ -261,14 +258,14 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
// Current limiting
bool limited = false;
float Ilim = axis_->motor_.effective_current_lim();
float Ilim = axis_->motor_.effective_current_lim();
if (Iq > Ilim) {
limited = true;
Iq = Ilim;
Iq = Ilim;
}
if (Iq < -Ilim) {
limited = true;
Iq = -Ilim;
Iq = -Ilim;
}
// Velocity integrator (behaviour dependent on limiting)
Firmware/MotorControl/controller.hpp
+52 -55
View File
@@ -6,24 +6,24 @@
#endif
class Controller {
public:
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_OVERSPEED = 0x01,
ERROR_NONE = 0,
ERROR_OVERSPEED = 0x01,
ERROR_INVALID_INPUT_MODE = 0x02,
ERROR_UNSTABLE_GAIN = 0x04,
ERROR_UNSTABLE_GAIN = 0x04,
};
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
enum ControlMode_t{
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
enum ControlMode_t {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
};
enum InputMode_t{
enum InputMode_t {
INPUT_MODE_INACTIVE,
INPUT_MODE_PASSTHROUGH,
INPUT_MODE_VEL_RAMP,
@@ -35,27 +35,27 @@ public:
typedef struct {
uint32_t index = 0;
float cogging_map[3600];
bool pre_calibrated = false;
bool calib_anticogging = false;
bool pre_calibrated = false;
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
float cogging_ratio = 1.0f;
} Anticogging_t;
struct Config_t {
ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: ControlMode_t
InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
InputMode_t input_mode = INPUT_MODE_PASSTHROUGH; //see: InputMode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// 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]
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s]
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. 0.0f to disable
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
bool setpoints_in_cpr = false;
float inertia = 0.0f; // [A/(count/s^2)]
float input_filter_bandwidth = 2.0f; // [1/s]
float homing_speed = 2000.0f; // [counts/s]
Anticogging_t anticogging;
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
@@ -72,7 +72,7 @@ public:
void move_incremental(float displacement, bool from_goal_point);
bool home_axis();
// TODO: make this more similar to other calibration loops
void start_anticogging_calibration();
bool anticogging_calibration(float pos_estimate, float vel_estimate);
@@ -81,7 +81,7 @@ public:
bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: anticogging overhaul:
// - expose selected (all?) variables on protocol
@@ -95,18 +95,18 @@ public:
float vel_setpoint_ = 0.0f;
// float vel_setpoint = 800.0f; <sensorless example>
float vel_integrator_current_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
float input_pos_ = 0.0f;
float input_vel_ = 0.0f;
float input_current_ = 0.0f;
float input_pos_ = 0.0f;
float input_vel_ = 0.0f;
float input_current_ = 0.0f;
float input_filter_kp_ = 0.0f;
float input_filter_ki_ = 0.0f;
bool input_pos_updated_ = false;
uint32_t traj_start_loop_count_ = 0;
bool trajectory_done_ = true;
bool trajectory_done_ = true;
bool anticogging_valid_ = false;
@@ -115,7 +115,7 @@ public:
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("input_pos", &input_pos_,
[](void* ctx) { static_cast<Controller*>(ctx)->input_pos_updated(); }, this),
[](void* ctx) { static_cast<Controller*>(ctx)->input_pos_updated(); }, this),
make_protocol_property("input_vel", &input_vel_),
make_protocol_property("input_current", &input_current_),
make_protocol_ro_property("pos_setpoint", &pos_setpoint_),
@@ -127,34 +127,31 @@ public:
make_protocol_property("gain_scheduling_width", &config_.gain_scheduling_width),
make_protocol_property("enable_gain_scheduling", &config_.enable_gain_scheduling),
make_protocol_object("config",
make_protocol_property("control_mode", &config_.control_mode),
make_protocol_property("input_mode", &config_.input_mode),
make_protocol_property("pos_gain", &config_.pos_gain),
make_protocol_property("vel_gain", &config_.vel_gain),
make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance),
make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate),
make_protocol_property("homing_speed", &config_.homing_speed),
make_protocol_property("inertia", &config_.inertia),
make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth,
[](void* ctx) { static_cast<Controller*>(ctx)->update_filter_gains(); }, this),
make_protocol_object("anticogging",
make_protocol_ro_property("index", &config_.anticogging.index),
make_protocol_property("pre_calibrated", &config_.anticogging.pre_calibrated),
make_protocol_ro_property("calib_anticogging", &config_.anticogging.calib_anticogging),
make_protocol_property("calib_pos_threshold", &config_.anticogging.calib_pos_threshold),
make_protocol_property("calib_vel_threshold", &config_.anticogging.calib_vel_threshold),
make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio)
)
),
make_protocol_property("control_mode", &config_.control_mode),
make_protocol_property("input_mode", &config_.input_mode),
make_protocol_property("pos_gain", &config_.pos_gain),
make_protocol_property("vel_gain", &config_.vel_gain),
make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("vel_limit_tolerance", &config_.vel_limit_tolerance),
make_protocol_property("vel_ramp_rate", &config_.vel_ramp_rate),
make_protocol_property("homing_speed", &config_.homing_speed),
make_protocol_property("inertia", &config_.inertia),
make_protocol_property("input_filter_bandwidth", &config_.input_filter_bandwidth,
[](void* ctx) { static_cast<Controller*>(ctx)->update_filter_gains(); }, this),
make_protocol_object("anticogging",
make_protocol_ro_property("index", &config_.anticogging.index),
make_protocol_property("pre_calibrated", &config_.anticogging.pre_calibrated),
make_protocol_ro_property("calib_anticogging", &config_.anticogging.calib_anticogging),
make_protocol_property("calib_pos_threshold", &config_.anticogging.calib_pos_threshold),
make_protocol_property("calib_vel_threshold", &config_.anticogging.calib_vel_threshold),
make_protocol_ro_property("cogging_ratio", &config_.anticogging.cogging_ratio))),
make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration),
make_protocol_function("home_axis", *this, &Controller::home_axis)
);
make_protocol_function("home_axis", *this, &Controller::home_axis));
}
};
DEFINE_ENUM_FLAG_OPERATORS(Controller::Error_t)
#endif // __CONTROLLER_HPP
#endif // __CONTROLLER_HPP
Firmware/MotorControl/encoder.cpp
+118 -111
View File
@@ -1,12 +1,9 @@
#include "odrive_main.h"
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config) :
hw_config_(hw_config),
config_(config)
{
Config_t& config) : hw_config_(hw_config),
config_(config) {
update_pll_gains();
if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) {
@@ -22,7 +19,7 @@ void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
set_idx_subscribe();
if(config_.mode & MODE_FLAG_ABS){
if (config_.mode & MODE_FLAG_ABS) {
abs_spi_cs_pin_init();
abs_spi_init();
}
@@ -33,7 +30,7 @@ void Encoder::set_error(Error_t error) {
axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
}
bool Encoder::do_checks(){
bool Encoder::do_checks() {
return error_ == ERROR_NONE;
}
@@ -48,10 +45,10 @@ void Encoder::enc_index_cb() {
if (config_.use_index) {
set_circular_count(0, false);
if (config_.zero_count_on_find_idx)
set_linear_count(0); // Avoid position control transient after search
set_linear_count(0); // Avoid position control transient after search
if (config_.pre_calibrated) {
is_ready_ = true;
if(axis_->controller_.config_.anticogging.pre_calibrated){
if (axis_->controller_.config_.anticogging.pre_calibrated) {
axis_->controller_.anticogging_valid_ = true;
}
} else {
@@ -70,15 +67,15 @@ void Encoder::enc_index_cb() {
void Encoder::set_idx_subscribe(bool override_enable) {
if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
enc_index_cb_wrapper, this);
enc_index_cb_wrapper, this);
} else if (!config_.use_index || config_.find_idx_on_lockin_only) {
GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
}
}
void Encoder::update_pll_gains() {
pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
pll_kp_ = 2.0f * config_.bandwidth; // basic conversion to discrete time
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
@@ -99,8 +96,8 @@ void Encoder::set_linear_count(int32_t count) {
uint32_t prim = cpu_enter_critical();
// Update states
shadow_count_ = count;
pos_estimate_ = (float)count;
shadow_count_ = count;
pos_estimate_ = (float)count;
tim_cnt_sample_ = count;
//Write hardware last
@@ -122,14 +119,14 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = (float)count_in_cpr_;
pos_cpr_ = (float)count_in_cpr_;
cpu_exit_critical(prim);
}
bool Encoder::run_index_search() {
config_.use_index = true;
index_found_ = false;
index_found_ = false;
if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
axis_->motor_.config_.direction = 1;
}
@@ -140,11 +137,11 @@ bool Encoder::run_index_search() {
}
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance;
int32_t init_enc_val = shadow_count_;
bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance;
axis_->config_.calibration_lockin.finish_on_distance = true;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance;
if (status) {
@@ -169,7 +166,7 @@ bool Encoder::run_direction_find() {
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -191,9 +188,9 @@ bool Encoder::run_offset_calibration() {
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
axis_->run_control_loop([&]() {
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
@@ -201,20 +198,20 @@ bool Encoder::run_offset_calibration() {
return false;
int32_t init_enc_val = shadow_count_;
int64_t encvaluesum = 0;
int64_t encvaluesum = 0;
// scan forward
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
@@ -235,35 +232,34 @@ bool Encoder::run_offset_calibration() {
//TODO avoid recomputing elec_rad_per_enc every time
// Check CPR
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = fabsf(shadow_count_-init_enc_val);
if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
calib_scan_response_ = fabsf(shadow_count_ - init_enc_val);
if (fabsf(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_OUT_OF_RANGE);
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
return false;
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.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
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.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;
return true;
@@ -271,13 +267,26 @@ bool Encoder::run_offset_calibration() {
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;
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;
}
}
@@ -297,37 +306,36 @@ void Encoder::sample_now() {
} break;
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:
{
case MODE_SPI_ABS_CUI: {
// Do nothing
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
} break;
}
}
bool Encoder::abs_spi_init(){
bool Encoder::abs_spi_init() {
if ((config_.mode & MODE_FLAG_ABS) == 0x0)
return false;
uint32_t cr1,cr2;
uint32_t cr1, cr2;
cr1 = hw_config_.spi->Instance->CR1;
cr2 = hw_config_.spi->Instance->CR2;
SPI_HandleTypeDef * spi = hw_config_.spi;
spi->Init.Mode = SPI_MODE_MASTER;
spi->Init.Direction = SPI_DIRECTION_2LINES;
spi->Init.DataSize = SPI_DATASIZE_16BIT;
spi->Init.CLKPolarity = SPI_POLARITY_LOW;
spi->Init.CLKPhase = SPI_PHASE_2EDGE;
spi->Init.NSS = SPI_NSS_SOFT;
SPI_HandleTypeDef* spi = hw_config_.spi;
spi->Init.Mode = SPI_MODE_MASTER;
spi->Init.Direction = SPI_DIRECTION_2LINES;
spi->Init.DataSize = SPI_DATASIZE_16BIT;
spi->Init.CLKPolarity = SPI_POLARITY_LOW;
spi->Init.CLKPhase = SPI_PHASE_2EDGE;
spi->Init.NSS = SPI_NSS_SOFT;
spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
spi->Init.TIMode = SPI_TIMODE_DISABLE;
spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spi->Init.CRCPolynomial = 10;
HAL_SPI_DeInit(spi);
HAL_SPI_Init(spi);
@@ -340,9 +348,9 @@ bool Encoder::abs_spi_init(){
return true;
}
bool Encoder::abs_spi_start_transaction(){
if (config_.mode & MODE_FLAG_ABS){
if(hw_config_.spi->State != HAL_SPI_STATE_READY){
bool Encoder::abs_spi_start_transaction() {
if (config_.mode & MODE_FLAG_ABS) {
if (hw_config_.spi->State != HAL_SPI_STATE_READY) {
set_error(ERROR_ABS_SPI_NOT_READY);
return false;
}
@@ -350,54 +358,54 @@ bool Encoder::abs_spi_start_transaction(){
hw_config_.spi->Instance->CR1 = abs_spi_cr1;
hw_config_.spi->Instance->CR2 = abs_spi_cr2;
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET);
HAL_SPI_TransmitReceive_DMA(hw_config_.spi,(uint8_t*)abs_spi_dma_tx_,(uint8_t*)abs_spi_dma_rx_,1);
HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1);
}
return true;
}
uint8_t parity(uint16_t v){
uint8_t parity(uint16_t v) {
v ^= v >> 8;
v ^= v >> 4;
v ^= v >> 2;
v ^= v >> 1;
return v & 1;
}
void Encoder::abs_spi_cb(){
void Encoder::abs_spi_cb() {
HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
switch (config_.mode) {
case MODE_SPI_ABS_AMS: {
uint8_t parity_calc, parity_bit;
parity_calc = parity(abs_spi_dma_rx_[0]&0x7FFF);
parity_bit = abs_spi_dma_rx_[0] >>15;
uint8_t parity_calc, parity_bit;
parity_calc = parity(abs_spi_dma_rx_[0] & 0x7FFF);
parity_bit = abs_spi_dma_rx_[0] >> 15;
if(parity_calc == parity_bit){
pos_abs_ = abs_spi_dma_rx_[0] & 0x3FFF;
// We are going to ignore values all high or low
// This might happen in normal operation, but its unlikely
// The filter will handle these cases
if(pos_abs_ != 0 && pos_abs_ != 0x3FFF)
abs_spi_pos_updated_ = true;
}
}break;
if (parity_calc == parity_bit) {
pos_abs_ = abs_spi_dma_rx_[0] & 0x3FFF;
// We are going to ignore values all high or low
// This might happen in normal operation, but its unlikely
// The filter will handle these cases
if (pos_abs_ != 0 && pos_abs_ != 0x3FFF)
abs_spi_pos_updated_ = true;
}
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
} break;
}
is_ready_ = true;
}
void Encoder::abs_spi_cs_pin_init(){
void Encoder::abs_spi_cs_pin_init() {
// Decode cs pin
abs_spi_cs_port_ = get_gpio_port_by_pin(config_.abs_spi_cs_gpio_pin);
abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin);
abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin);
// Init cs pin
HAL_GPIO_DeInit(abs_spi_cs_port_, abs_spi_cs_pin_);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = abs_spi_cs_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Pin = abs_spi_cs_pin_;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(abs_spi_cs_port_, &GPIO_InitStruct);
@@ -414,7 +422,7 @@ bool Encoder::update() {
//TODO: use count_in_cpr_ instead as shadow_count_ can overflow
//or use 64 bit
int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
delta_enc = (int32_t)delta_enc_16; //sign extend
delta_enc = (int32_t)delta_enc_16; //sign extend
} break;
case MODE_HALL: {
@@ -433,41 +441,40 @@ bool Encoder::update() {
} break;
case MODE_SINCOS: {
float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
int fake_count = (int)(1000.0f * phase);
//CPR = 6283 = 2pi * 1k
delta_enc = fake_count - count_in_cpr_;
delta_enc = mod(delta_enc, 6283);
if (delta_enc > 6283/2)
if (delta_enc > 6283 / 2)
delta_enc -= 6283;
} break;
case MODE_SPI_ABS_AMS:
case MODE_SPI_ABS_CUI:{
if(abs_spi_pos_updated_ == false && abs_spi_pos_init_once_){
case MODE_SPI_ABS_CUI: {
if (abs_spi_pos_updated_ == false && abs_spi_pos_init_once_) {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
// if (spi_error_rate_ > 0.005f)
// set_error(ERROR_ABS_SPI_COM_FAIL);
}
else
} else
// Low pass filter the error
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
abs_spi_pos_updated_ = false;
delta_enc = pos_abs_ - count_in_cpr_;
delta_enc = mod(delta_enc, config_.cpr);
if (delta_enc > config_.cpr/2)
delta_enc = pos_abs_ - count_in_cpr_;
delta_enc = mod(delta_enc, config_.cpr);
if (delta_enc > config_.cpr / 2)
delta_enc -= config_.cpr;
if(!abs_spi_pos_init_once_ && delta_enc != 0){
if (!abs_spi_pos_init_once_ && delta_enc != 0) {
abs_spi_pos_init_once_ = true;
}
}break;
} break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return false;
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
return false;
} break;
}
@@ -475,25 +482,25 @@ bool Encoder::update() {
count_in_cpr_ += delta_enc;
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
if(config_.mode & MODE_FLAG_ABS)
if (config_.mode & MODE_FLAG_ABS)
count_in_cpr_ = pos_abs_;
//// run pll (for now pll is in units of encoder counts)
// Predict current pos
pos_estimate_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
// 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));
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));
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
bool snap_to_zero_vel = false;
if (fabsf(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) {
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
vel_estimate_ = 0.0f; //align delta-sigma on zero to prevent jitter
snap_to_zero_vel = true;
}
@@ -502,7 +509,7 @@ bool Encoder::update() {
// if we are stopped, make sure we don't randomly drift
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
interpolation_ = 0.5f;
// reset interpolation if encoder edge comes
// reset interpolation if encoder edge comes
} else if (delta_enc > 0) {
interpolation_ = 0.0f;
} else if (delta_enc < 0) {
@@ -519,7 +526,7 @@ bool Encoder::update() {
//// 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);
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
Firmware/MotorControl/encoder.hpp
+75 -77
View File
@@ -6,18 +6,18 @@
#endif
class Encoder {
public:
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_OUT_OF_RANGE = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_OUT_OF_RANGE = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
ERROR_ILLEGAL_HALL_STATE = 0x10,
ERROR_INDEX_NOT_FOUND_YET = 0x20,
ERROR_ABS_SPI_TIMEOUT = 0x40,
ERROR_ABS_SPI_COM_FAIL = 0x80,
ERROR_ABS_SPI_NOT_READY = 0x100,
ERROR_ILLEGAL_HALL_STATE = 0x10,
ERROR_INDEX_NOT_FOUND_YET = 0x20,
ERROR_ABS_SPI_TIMEOUT = 0x40,
ERROR_ABS_SPI_COM_FAIL = 0x80,
ERROR_ABS_SPI_NOT_READY = 0x100,
};
enum Mode_t {
@@ -31,30 +31,30 @@ public:
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.
bool zero_count_on_find_idx = true;
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // Offset between encoder count and rotor electrical phase
float offset_float = 0.0f; // Sub-count phase alignment offset
bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
float calib_scan_distance = 16.0f * M_PI; // rad electrical
float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
float bandwidth = 1000.0f;
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
bool idx_search_unidirectional = false; // Only allow index search in known direction
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
uint16_t abs_spi_cs_gpio_pin = 0;
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.
bool zero_count_on_find_idx = true;
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // Offset between encoder count and rotor electrical phase
float offset_float = 0.0f; // Sub-count phase alignment offset
bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
float calib_scan_distance = 16.0f * M_PI; // rad electrical
float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
float bandwidth = 1000.0f;
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
bool idx_search_unidirectional = false; // Only allow index search in known direction
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
uint16_t abs_spi_cs_gpio_pin = 0;
};
Encoder(const EncoderHardwareConfig_t& hw_config,
Config_t& config);
Config_t& config);
void setup();
void set_error(Error_t error);
bool do_checks();
@@ -76,27 +76,27 @@ public:
const EncoderHardwareConfig_t& hw_config_;
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Axis* axis_ = nullptr; // set by Axis constructor
Error_t error_ = ERROR_NONE;
bool index_found_ = false;
bool is_ready_ = false;
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
float interpolation_ = 0.0f;
float phase_ = 0.0f; // [count]
float pos_estimate_ = 0.0f; // [count]
float pos_cpr_ = 0.0f; // [count]
float vel_estimate_ = 0.0f; // [count/s]
float pll_kp_ = 0.0f; // [count/s / count]
float pll_ki_ = 0.0f; // [(count/s^2) / count]
float calib_scan_response_ = 0.0f; // debug report from offset calib
int32_t pos_abs_ = 0;
float spi_error_rate_ = 0.0f;
Error_t error_ = ERROR_NONE;
bool index_found_ = false;
bool is_ready_ = false;
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
float interpolation_ = 0.0f;
float phase_ = 0.0f; // [count]
float pos_estimate_ = 0.0f; // [count]
float pos_cpr_ = 0.0f; // [count]
float vel_estimate_ = 0.0f; // [count/s]
float pll_kp_ = 0.0f; // [count/s / count]
float pll_ki_ = 0.0f; // [(count/s^2) / count]
float calib_scan_response_ = 0.0f; // debug report from offset calib
int32_t pos_abs_ = 0;
float spi_error_rate_ = 0.0f;
int16_t tim_cnt_sample_ = 0; //
int16_t tim_cnt_sample_ = 0; //
// Updated by low_level pwm_adc_cb
uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
float sincos_sample_s_ = 0.0f;
float sincos_sample_c_ = 0.0f;
@@ -106,14 +106,14 @@ public:
void abs_spi_cs_pin_init();
uint16_t abs_spi_dma_tx_[2] = {0xFFFF, 0x0000};
uint16_t abs_spi_dma_rx_[2];
bool abs_spi_pos_updated_ = false;
bool abs_spi_pos_updated_ = false;
bool abs_spi_pos_init_once_ = false;
GPIO_TypeDef* abs_spi_cs_port_;
uint16_t abs_spi_cs_pin_;
uint32_t abs_spi_cr1;
uint32_t abs_spi_cr2;
constexpr float getCoggingRatio(){
constexpr float getCoggingRatio() {
return config_.cpr / 3600.0f;
}
@@ -136,34 +136,32 @@ public:
make_protocol_ro_property("spi_error_rate", &spi_error_rate_),
make_protocol_object("config",
make_protocol_property("mode", &config_.mode,
[](void* ctx) { static_cast<Encoder*>(ctx)->abs_spi_init(); }, this),
make_protocol_property("use_index", &config_.use_index,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("pre_calibrated", &config_.pre_calibrated,
[](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin,
[](void* ctx) { static_cast<Encoder*>(ctx)->abs_spi_cs_pin_init(); }, this),
make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("offset_float", &config_.offset_float),
make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
make_protocol_property("bandwidth", &config_.bandwidth,
[](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
make_protocol_property("calib_range", &config_.calib_range),
make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
),
make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count")
);
make_protocol_property("mode", &config_.mode,
[](void* ctx) { static_cast<Encoder*>(ctx)->abs_spi_init(); }, this),
make_protocol_property("use_index", &config_.use_index,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
[](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
make_protocol_property("pre_calibrated", &config_.pre_calibrated,
[](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
make_protocol_property("abs_spi_cs_gpio_pin", &config_.abs_spi_cs_gpio_pin,
[](void* ctx) { static_cast<Encoder*>(ctx)->abs_spi_cs_pin_init(); }, this),
make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("offset_float", &config_.offset_float),
make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
make_protocol_property("bandwidth", &config_.bandwidth,
[](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
make_protocol_property("calib_range", &config_.calib_range),
make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)),
make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count"));
}
};
DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t)
#endif // __ENCODER_HPP
#endif // __ENCODER_HPP
Firmware/MotorControl/endstop.cpp
+9 -9
View File
@@ -6,18 +6,18 @@ Endstop::Endstop(Endstop::Config_t& config)
}
void Endstop::update() {
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
auto last_pin_state = pin_state_;
pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
auto last_pin_state = pin_state_;
pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
if (pin_state_ != last_pin_state) {
debounce_timer_ = axis_->loop_counter_ * current_meas_period;
}
if (config_.enabled) {
float now = axis_->loop_counter_ * current_meas_period;
if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state
endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state
debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues
if ((now - debounce_timer_) >= (config_.debounce_ms * 0.001f)) { // Debounce timer expired, take the new pin state
endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_; // endstop_state is the logical state
debounce_timer_ = now - (config_.debounce_ms * 0.001f); // Ensure timer doesn't have overflow issues
} else {
endstop_state_ = endstop_state_; // Do nothing
}
@@ -30,18 +30,18 @@ bool Endstop::getEndstopState() {
return endstop_state_;
}
void Endstop::update_endstop_config(){
void Endstop::update_endstop_config() {
set_endstop_enabled(config_.enabled);
}
void Endstop::set_endstop_enabled(bool enable) {
if (config_.gpio_num != 0) {
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
if (enable) {
HAL_GPIO_DeInit(gpio_port, gpio_pin);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
Firmware/MotorControl/endstop.hpp
+4 -4
View File
@@ -5,10 +5,10 @@ class Endstop {
public:
struct Config_t {
uint16_t gpio_num;
bool enabled = false;
int32_t offset = 0;
bool enabled = false;
int32_t offset = 0;
bool is_active_high = false;
float debounce_ms = 100.0f;
float debounce_ms = 100.0f;
};
Endstop(Endstop::Config_t& config);
@@ -38,7 +38,7 @@ class Endstop {
}
private:
bool pin_state_ = false;
bool pin_state_ = false;
volatile float debounce_timer_ = 0;
};
#endif
Firmware/MotorControl/low_level.cpp
+83 -89
View File
@@ -28,21 +28,21 @@
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
const float adc_full_scale = (float)(1 << 12);
const float adc_full_scale = (float)(1 << 12);
const float adc_ref_voltage = 3.3f;
/* 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;
float vbus_voltage = 12.0f;
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]);
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];
static uint16_t GPIO_port_samples[2][num_GPIO];
/* CPU critical section helpers ----------------------------------------------*/
/* Safety critical functions -------------------------------------------------*/
@@ -109,8 +109,8 @@ void safety_critical_arm_motor_pwm(Motor& motor) {
// safety_critical_arm_motor_phases is called.
// @returns true if the motor was in a state other than disarmed before
bool safety_critical_disarm_motor_pwm(Motor& motor) {
uint32_t mask = cpu_enter_critical();
bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
uint32_t mask = cpu_enter_critical();
bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer);
cpu_exit_critical(mask);
@@ -154,7 +154,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() {
uint32_t mask = cpu_enter_critical();
uint32_t mask = cpu_enter_critical();
brake_resistor_armed = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
@@ -166,7 +166,7 @@ void safety_critical_arm_brake_resistor() {
// After calling this, the brake resistor can only be armed again
// by calling safety_critical_arm_brake_resistor().
void safety_critical_disarm_brake_resistor() {
uint32_t mask = cpu_enter_critical();
uint32_t mask = cpu_enter_critical();
brake_resistor_armed = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
@@ -217,7 +217,7 @@ void start_adc_pwm() {
start_pwm(&htim8);
// TODO: explain why this offset
sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128,
&htim13);
&htim13);
// Motor output starts in the disabled state
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
@@ -242,7 +242,7 @@ void start_adc_pwm() {
void start_pwm(TIM_HandleTypeDef* htim) {
// Init PWM
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
htim->Instance->CCR1 = half_load;
htim->Instance->CCR2 = half_load;
htim->Instance->CCR3 = half_load;
@@ -265,8 +265,8 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
// Store intial timer configs
uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t CR2_store = htim_a->Instance->CR2;
uint16_t SMCR_store = htim_b->Instance->SMCR;
uint16_t CR2_store = htim_a->Instance->CR2;
uint16_t SMCR_store = htim_b->Instance->SMCR;
// Turn off output
htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE);
htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE);
@@ -299,12 +299,12 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
// Set and start reference timebase timer (if used)
if (htim_refbase) {
htim_refbase->Instance->CNT = count_offset;
htim_refbase->Instance->CR1 |= (TIM_CR1_CEN); // start
htim_refbase->Instance->CR1 |= (TIM_CR1_CEN); // start
}
// Start Timer a
htim_a->Instance->CR1 |= (TIM_CR1_CEN);
// Restore timer configs
htim_a->Instance->CR2 = CR2_store;
htim_a->Instance->CR2 = CR2_store;
htim_b->Instance->SMCR = SMCR_store;
// restore output
htim_a->Instance->BDTR |= MOE_store_a;
@@ -312,7 +312,7 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
}
// @brief ADC1 measurements are written to this buffer by DMA
uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = { 0 };
uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = {0};
// @brief Starts the general purpose ADC on the ADC1 peripheral.
// The measured ADC voltages can be read with get_adc_voltage().
@@ -327,20 +327,19 @@ void start_general_purpose_adc() {
ADC_ChannelConfTypeDef sConfig;
// Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = ENABLE;
hadc1.Init.ContinuousConvMode = ENABLE;
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = ENABLE;
hadc1.Init.ContinuousConvMode = ENABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT;
hadc1.Init.DMAContinuousRequests = ENABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc1) != HAL_OK) {
_Error_Handler((char*)__FILE__, __LINE__);
}
@@ -348,7 +347,7 @@ void start_general_purpose_adc() {
sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES;
for (uint32_t channel = 0; channel < ADC_CHANNEL_COUNT; ++channel) {
sConfig.Channel = channel << ADC_CR1_AWDCH_Pos;
sConfig.Rank = channel + 1; // rank numbering starts at 1
sConfig.Rank = channel + 1; // rank numbering starts at 1
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
_Error_Handler((char*)__FILE__, __LINE__);
}
@@ -414,7 +413,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
if (channel < ADC_CHANNEL_COUNT)
return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
else
return 0.0f / 0.0f; // NaN
return 0.0f / 0.0f; // NaN
}
//--------------------------------
@@ -425,7 +424,7 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
static const float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
// Only one conversion in sequence, so only rank1
uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
vbus_voltage = ADCValue * voltage_scale;
vbus_voltage = ADCValue * voltage_scale;
if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) {
if (oscilloscope_pos >= OSCILLOSCOPE_SIZE)
oscilloscope_pos = 0;
@@ -478,10 +477,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
// Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
// 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;
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;
bool current_meas_not_DC_CAL = !counting_down;
// Check the timing of the sequencing
@@ -493,12 +492,12 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
bool update_timings = false;
if (hadc == &hadc2) {
if (&axis == axes[1] && counting_down)
update_timings = true; // update timings of M0
update_timings = true; // update timings of M0
else if (&axis == axes[0] && !counting_down)
update_timings = true; // update timings of M1
update_timings = true; // update timings of M1
if((current_meas_not_DC_CAL && !axis_num) ||
(axis_num && !current_meas_not_DC_CAL)){
if ((current_meas_not_DC_CAL && !axis_num) ||
(axis_num && !current_meas_not_DC_CAL)) {
axis.encoder_.abs_spi_start_transaction();
}
}
@@ -515,8 +514,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
} else {
other_axis.motor_.next_timings_valid_ = false;
safety_critical_apply_motor_pwm_timings(
other_axis.motor_, other_axis.motor_.next_timings_
);
other_axis.motor_, other_axis.motor_.next_timings_);
}
update_brake_current();
}
@@ -559,21 +557,20 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
}
void tim_update_cb(TIM_HandleTypeDef* htim) {
// 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
bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR;
if (counting_down)
return;
int sample_ch;
Axis* axis;
if (htim == &htim1) {
sample_ch = 0;
axis = axes[0];
axis = axes[0];
} else if (htim == &htim8) {
sample_ch = 1;
axis = axes[1];
axis = axes[1];
} else {
low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK);
return;
@@ -595,10 +592,10 @@ void update_brake_current() {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
}
// Don't start braking until -Ibus > regen_current_allowed
float brake_current = std::max(-Ibus_sum - board_config.max_regen_current, 0.0f);
float brake_duty = std::max(brake_current * std::abs(board_config.brake_resistance) / vbus_voltage, 0.0f);
float brake_current = std::max(-Ibus_sum - board_config.max_regen_current, 0.0f);
float brake_duty = std::max(brake_current * std::abs(board_config.brake_resistance) / vbus_voltage, 0.0f);
// Duty limit at 90% to allow bootstrap caps to charge
// If brake_duty is NaN, this expression will also evaluate to false
@@ -613,7 +610,6 @@ void update_brake_current() {
}
}
/* RC PWM input --------------------------------------------------------------*/
// @brief Returns the ODrive GPIO number for a given
@@ -641,11 +637,16 @@ int tim_2_5_channel_num_to_gpio_num(int channel) {
uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) {
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
switch (gpio_num) {
case 1: return TIM_CHANNEL_1;
case 2: return TIM_CHANNEL_2;
case 3: return TIM_CHANNEL_3;
case 4: return TIM_CHANNEL_4;
default: return 0;
case 1:
return TIM_CHANNEL_1;
case 2:
return TIM_CHANNEL_2;
case 3:
return TIM_CHANNEL_3;
case 4:
return TIM_CHANNEL_4;
default:
return 0;
}
#else
// Only ch4 is available on v3.2
@@ -659,21 +660,22 @@ uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) {
void pwm_in_init() {
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
TIM_IC_InitTypeDef sConfigIC;
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
sConfigIC.ICFilter = 15;
sConfigIC.ICFilter = 15;
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
for (int gpio_num = 1; gpio_num <= 4; ++gpio_num) {
#else
int gpio_num = 4; {
int gpio_num = 4;
{
#endif
if (is_endpoint_ref_valid(board_config.pwm_mappings[gpio_num - 1].endpoint)) {
GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num);
@@ -686,12 +688,12 @@ void pwm_in_init() {
}
//TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz
#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ
#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse
#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward
#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms
#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms
#define PWM_INVERT_INPUT false
#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ
#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse
#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward
#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms
#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms
#define PWM_INVERT_INPUT false
void handle_pulse(int gpio_num, uint32_t high_time) {
if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME)
@@ -702,7 +704,7 @@ void handle_pulse(int gpio_num, uint32_t high_time) {
if (high_time > PWM_MAX_HIGH_TIME)
high_time = PWM_MAX_HIGH_TIME;
float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME);
float value = board_config.pwm_mappings[gpio_num - 1].min +
float value = board_config.pwm_mappings[gpio_num - 1].min +
(fraction * (board_config.pwm_mappings[gpio_num - 1].max - board_config.pwm_mappings[gpio_num - 1].min));
Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num - 1].endpoint);
@@ -713,41 +715,36 @@ void handle_pulse(int gpio_num, uint32_t high_time) {
}
void pwm_in_cb(int channel, uint32_t timestamp) {
static uint32_t last_timestamp[GPIO_COUNT] = { 0 };
static bool last_pin_state[GPIO_COUNT] = { false };
static bool last_sample_valid[GPIO_COUNT] = { false };
static uint32_t last_timestamp[GPIO_COUNT] = {0};
static bool last_pin_state[GPIO_COUNT] = {false};
static bool last_sample_valid[GPIO_COUNT] = {false};
int gpio_num = tim_2_5_channel_num_to_gpio_num(channel);
if (gpio_num < 1 || gpio_num > GPIO_COUNT)
return;
bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET;
if (last_sample_valid[gpio_num - 1]
&& (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT)
&& (current_pin_state == PWM_INVERT_INPUT)) {
if (last_sample_valid[gpio_num - 1] && (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT) && (current_pin_state == PWM_INVERT_INPUT)) {
handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num - 1]);
}
last_timestamp[gpio_num - 1] = timestamp;
last_pin_state[gpio_num - 1] = current_pin_state;
last_timestamp[gpio_num - 1] = timestamp;
last_pin_state[gpio_num - 1] = current_pin_state;
last_sample_valid[gpio_num - 1] = true;
}
/* Analog speed control input */
static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio)
{
static void update_analog_endpoint(const struct PWMMapping_t* map, int gpio) {
float fraction = get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio)) / 3.3f;
float value = map->min + (fraction * (map->max - map->min));
float value = map->min + (fraction * (map->max - map->min));
get_endpoint(map->endpoint)->set_from_float(value);
}
static void analog_polling_thread(void *)
{
static void analog_polling_thread(void*) {
while (true) {
for (int i = 0; i < GPIO_COUNT; i++) {
struct PWMMapping_t *map = &board_config.analog_mappings[i];
struct PWMMapping_t* map = &board_config.analog_mappings[i];
if (is_endpoint_ref_valid(map->endpoint))
update_analog_endpoint(map, i + 1);
@@ -756,16 +753,13 @@ static void analog_polling_thread(void *)
}
}
void start_analog_thread()
{
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4*512);
void start_analog_thread() {
osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 4 * 512);
osThreadCreate(osThread(thread_def), NULL);
}
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
if(hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_)
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef* hspi) {
if (hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_)
axes[0]->encoder_.abs_spi_cb();
else if (hspi->pRxBuffPtr == (uint8_t*)axes[1]->encoder_.abs_spi_dma_rx_)
axes[1]->encoder_.abs_spi_cb();
Firmware/MotorControl/low_level.h
+2 -2
View File
@@ -11,9 +11,9 @@ extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <adc.h>
#include <cmsis_os.h>
#include <stdbool.h>
#include <adc.h>
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
@@ -69,4 +69,4 @@ inline void cpu_exit_critical(uint32_t priority_mask) {
}
#endif
#endif //__LOW_LEVEL_H
#endif //__LOW_LEVEL_H
Firmware/MotorControl/main.cpp
+46 -43
View File
@@ -1,13 +1,13 @@
#define __MAIN_CPP__
#include "odrive_main.h"
#include "nvm_config.hpp"
#include "odrive_main.h"
#include "freertos_vars.h"
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
#include <communication/interface_i2c.h>
#include <communication/interface_uart.h>
#include <communication/interface_usb.h>
#include <communication/interface_can.hpp>
#include "freertos_vars.h"
BoardConfig_t board_config;
ODriveCAN::Config_t can_config;
@@ -21,7 +21,7 @@ Endstop::Config_t min_endstop_configs[AXIS_COUNT];
Endstop::Config_t max_endstop_configs[AXIS_COUNT];
bool user_config_loaded_;
SystemStats_t system_stats_ = { 0 };
SystemStats_t system_stats_ = {0};
Axis *axes[AXIS_COUNT];
ODriveCAN *odCAN;
@@ -36,7 +36,8 @@ typedef Config<
TrapezoidalTrajectory::Config_t[AXIS_COUNT],
Endstop::Config_t[AXIS_COUNT],
Endstop::Config_t[AXIS_COUNT],
Axis::Config_t[AXIS_COUNT]> ConfigFormat;
Axis::Config_t[AXIS_COUNT]>
ConfigFormat;
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
@@ -50,7 +51,8 @@ void save_configuration(void) {
&min_endstop_configs,
&max_endstop_configs,
&axis_configs)) {
printf("saving configuration failed\r\n"); osDelay(5);
printf("saving configuration failed\r\n");
osDelay(5);
} else {
user_config_loaded_ = true;
}
@@ -60,26 +62,26 @@ extern "C" int load_configuration(void) {
// Try to load configs
if (NVM_init() ||
ConfigFormat::safe_load_config(
&board_config,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
&motor_configs,
&trap_configs,
&min_endstop_configs,
&max_endstop_configs,
&axis_configs)) {
&board_config,
&can_config,
&encoder_configs,
&sensorless_configs,
&controller_configs,
&motor_configs,
&trap_configs,
&min_endstop_configs,
&max_endstop_configs,
&axis_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
can_config = ODriveCAN::Config_t();
can_config = ODriveCAN::Config_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = Encoder::Config_t();
encoder_configs[i] = Encoder::Config_t();
sensorless_configs[i] = SensorlessEstimator::Config_t();
controller_configs[i] = Controller::Config_t();
motor_configs[i] = Motor::Config_t();
trap_configs[i] = TrapezoidalTrajectory::Config_t();
axis_configs[i] = Axis::Config_t();
motor_configs[i] = Motor::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]);
@@ -98,7 +100,8 @@ void erase_configuration(void) {
void enter_dfu_mode() {
if ((hw_version_major == 3) && (hw_version_minor >= 5)) {
__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
__asm volatile("CPSID I\n\t" ::
: "memory"); // disable interrupts
_reboot_cookie = 0xDEADBEEF;
NVIC_SystemReset();
} else {
@@ -118,26 +121,26 @@ extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
for (;;); // TODO: safe action
for (;;)
; // TODO: safe action
}
void vApplicationIdleHook(void) {
if (system_stats_.fully_booted) {
system_stats_.uptime = xTaskGetTickCount();
system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
system_stats_.uptime = xTaskGetTickCount();
system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
}
}
}
int odrive_main(void) {
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_i2c_instead_of_can) {
// Set up the direction GPIO as input
@@ -166,7 +169,7 @@ int odrive_main(void) {
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Pin = GPIO_1_Pin;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
@@ -182,20 +185,20 @@ int odrive_main(void) {
// Construct all objects.
odCAN = new ODriveCAN(&hcan1, can_config);
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
encoder_configs[i]);
SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]);
Controller *controller = new Controller(controller_configs[i]);
Motor *motor = new Motor(hw_configs[i].motor_config,
Controller *controller = new Controller(controller_configs[i]);
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor, *trap, *min_endstop, *max_endstop);
TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor, *trap, *min_endstop, *max_endstop);
}
// Start ADC for temperature measurements and user measurements
start_general_purpose_adc();
Firmware/MotorControl/motor.cpp
+70 -77
View File
@@ -4,20 +4,18 @@
#include "drv8301.h"
#include "odrive_main.h"
Motor::Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
Config_t& config) :
hw_config_(hw_config),
gate_driver_config_(gate_driver_config),
config_(config),
gate_driver_({
.spiHandle = gate_driver_config_.spi,
.EngpioHandle = gate_driver_config_.enable_port,
.EngpioNumber = gate_driver_config_.enable_pin,
.nCSgpioHandle = gate_driver_config_.nCS_port,
.nCSgpioNumber = gate_driver_config_.nCS_pin,
}) {
Config_t& config) : hw_config_(hw_config),
gate_driver_config_(gate_driver_config),
config_(config),
gate_driver_({
.spiHandle = gate_driver_config_.spi,
.EngpioHandle = gate_driver_config_.enable_port,
.EngpioNumber = gate_driver_config_.enable_pin,
.nCSgpioHandle = gate_driver_config_.nCS_port,
.nCSgpioNumber = gate_driver_config_.nCS_pin,
}) {
update_current_controller_gains();
}
@@ -33,7 +31,6 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config,
//
// @returns: True on success, false otherwise
bool Motor::arm() {
// Reset controller states, integrators, setpoints, etc.
axis_->controller_.reset();
reset_current_control();
@@ -58,7 +55,7 @@ void Motor::reset_current_control() {
void Motor::update_current_controller_gains() {
// Calculate current control gains
current_control_.p_gain = config_.current_control_bandwidth * config_.phase_inductance;
float plant_pole = config_.phase_resistance / config_.phase_inductance;
float plant_pole = config_.phase_resistance / config_.phase_inductance;
current_control_.i_gain = plant_pole * current_control_.p_gain;
}
@@ -72,29 +69,28 @@ void Motor::DRV8301_setup() {
// 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 kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer
static const float max_output_swing = 1.35f; // [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]
static const float kMargin = 0.90f;
static const float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer
static const float max_output_swing = 1.35f; // [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]
// Decoding array for snapping gain
std::array<std::pair<float, DRV8301_ShuntAmpGain_e>, 4> gain_choices = {
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)
};
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;
});
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;
if (gain_snap_down == gain_choices.crend())
--gain_snap_down;
// Values for current controller
phase_current_rev_gain_ = 1.0f / gain_snap_down->first;
@@ -111,7 +107,7 @@ void Motor::DRV8301_setup() {
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->Ctrl_Reg_2.GAIN = gain_snap_down->second;
local_regs->SndCmd = true;
DRV8301_writeData(&gate_driver_, local_regs);
@@ -136,7 +132,7 @@ bool Motor::check_DRV_fault() {
return true;
}
void Motor::set_error(Motor::Error_t error){
void Motor::set_error(Motor::Error_t error) {
error_ |= error;
axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
safety_critical_disarm_motor_pwm(*this);
@@ -144,17 +140,17 @@ void Motor::set_error(Motor::Error_t error){
}
float Motor::get_inverter_temp() {
float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch];
float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch];
float normalized_voltage = adc / adc_full_scale;
return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
}
bool Motor::update_thermal_limits() {
float fet_temp = get_inverter_temp();
float temp_margin = config_.inverter_temp_limit_upper - fet_temp;
float fet_temp = get_inverter_temp();
float temp_margin = config_.inverter_temp_limit_upper - fet_temp;
float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower;
thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range);
if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN
if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN
thermal_current_lim_ = 0.0f;
}
if (fet_temp > config_.inverter_temp_limit_upper + 5) {
@@ -181,7 +177,7 @@ float Motor::effective_current_lim() {
float current_lim = config_.current_lim;
// Hardware limit
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage);
current_lim = std::min(current_lim, 0.98f * one_by_sqrt3 * vbus_voltage);
} else {
current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current);
}
@@ -193,7 +189,7 @@ float Motor::effective_current_lim() {
void Motor::log_timing(TimingLog_t log_idx) {
static const uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config
uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config
if (log_idx < TIMING_LOG_NUM_SLOTS) {
timing_log_[log_idx] = timing;
@@ -201,10 +197,10 @@ void Motor::log_timing(TimingLog_t log_idx) {
}
float Motor::phase_current_from_adcval(uint32_t ADCValue) {
int adcval_bal = (int)ADCValue - (1 << 11);
int adcval_bal = (int)ADCValue - (1 << 11);
float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
float shunt_volt = amp_out_volt * phase_current_rev_gain_;
float current = shunt_volt * hw_config_.shunt_conductance;
float shunt_volt = amp_out_volt * phase_current_rev_gain_;
float current = shunt_volt * hw_config_.shunt_conductance;
return current;
}
@@ -214,12 +210,12 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) {
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
axis_->run_control_loop([&](){
axis_->run_control_loop([&]() {
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)
@@ -227,7 +223,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
// Test voltage along phase A
if (!enqueue_voltage_timings(test_voltage, 0.0f))
return false; // error set inside enqueue_voltage_timings
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_R);
return ++i < num_test_cycles;
@@ -239,24 +235,24 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
// return false; // error set inside enqueue_voltage_timings
float R = test_voltage / test_current;
float R = test_voltage / test_current;
config_.phase_resistance = R;
return true; // if we ran to completion that means success
return true; // if we ran to completion that means success
}
bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
float test_voltages[2] = {voltage_low, voltage_high};
float Ialphas[2] = {0.0f};
float test_voltages[2] = {voltage_low, voltage_high};
float Ialphas[2] = {0.0f};
static const int num_cycles = 5000;
size_t t = 0;
axis_->run_control_loop([&](){
axis_->run_control_loop([&]() {
int i = t & 1;
Ialphas[i] += -current_meas_.phB - current_meas_.phC;
// Test voltage along phase A
if (!enqueue_voltage_timings(test_voltages[i], 0.0f))
return false; // error set inside enqueue_voltage_timings
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_L);
return ++t < (num_cycles << 1);
@@ -272,7 +268,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
// Note: A more correct formula would also take into account that there is a finite timestep.
// However, the discretisation in the current control loop inverts the same discrepancy
float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles);
float L = v_L / dI_by_dt;
float L = v_L / dI_by_dt;
config_.phase_inductance = L;
// TODO arbitrary values set for now
@@ -281,7 +277,6 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
return true;
}
bool Motor::run_calibration() {
float R_calib_max_voltage = config_.resistance_calib_max_voltage;
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
@@ -296,7 +291,7 @@ bool Motor::run_calibration() {
}
update_current_controller_gains();
is_calibrated_ = true;
return true;
}
@@ -305,17 +300,17 @@ 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 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);
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);
next_timings_valid_ = true;
return true;
}
bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float mod_alpha = vfactor * v_alpha;
float mod_beta = vfactor * v_beta;
float mod_beta = vfactor * v_beta;
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false;
log_timing(TIMING_LOG_FOC_VOLTAGE);
@@ -324,10 +319,10 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
// We should probably make FOC Current call FOC Voltage to avoid duplication.
bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
float c = our_arm_cos_f32(pwm_phase);
float s = our_arm_sin_f32(pwm_phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
float c = our_arm_cos_f32(pwm_phase);
float s = our_arm_sin_f32(pwm_phase);
float v_alpha = c * v_d - s * v_q;
float v_beta = c * v_q + s * v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
@@ -339,21 +334,20 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
ictrl.Iq_setpoint = Iq_des;
// Check for current sense saturation
if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level
|| fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) {
if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level || fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) {
set_error(ERROR_CURRENT_SENSE_SATURATION);
return false;
}
// Clarke transform
float Ialpha = -current_meas_.phB - current_meas_.phC;
float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
// Park transform
float c_I = our_arm_cos_f32(I_phase);
float s_I = our_arm_sin_f32(I_phase);
float Id = c_I * Ialpha + s_I * Ibeta;
float Iq = c_I * Ibeta - s_I * Ialpha;
float Id = c_I * Ialpha + s_I * Ibeta;
float Iq = c_I * Ibeta - s_I * Ialpha;
ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured);
ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);
@@ -375,8 +369,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
float V_to_mod = 1.0f / mod_to_V;
float mod_d = V_to_mod * Vd;
float mod_q = V_to_mod * Vq;
float mod_d = V_to_mod * Vd;
float mod_q = V_to_mod * Vq;
// Vector modulation saturation, lock integrator if saturated
// TODO make maximum modulation configurable
@@ -396,24 +390,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
ictrl.Ibus = mod_d * Id + mod_q * Iq;
// Inverse park transform
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha = c_p * mod_d - s_p * mod_q;
float mod_beta = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorles estimator)
ictrl.final_v_alpha = mod_to_V * mod_alpha;
ictrl.final_v_beta = mod_to_V * mod_beta;
ictrl.final_v_beta = mod_to_V * mod_beta;
// Apply SVM
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false; // error set inside enqueue_modulation_timings
return false; // error set inside enqueue_modulation_timings
log_timing(TIMING_LOG_FOC_CURRENT);
return true;
}
bool Motor::update(float current_setpoint, float phase, float phase_vel) {
current_setpoint *= config_.direction;
phase *= config_.direction;
@@ -424,12 +417,12 @@ bool Motor::update(float current_setpoint, float phase, float phase_vel) {
// Execute current command
// TODO: move this into the mot
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){
if (!FOC_current(0.0f, current_setpoint, phase, pwm_phase)) {
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(0.0f, current_setpoint, pwm_phase))
if (!FOC_voltage(0.0f, current_setpoint, pwm_phase))
return false;
} else {
set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
Firmware/MotorControl/motor.hpp
+108 -113
View File
@@ -8,22 +8,22 @@
#include "drv8301.h"
class Motor {
public:
public:
enum Error_t {
ERROR_NONE = 0,
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,
ERROR_CURRENT_SENSE_SATURATION = 0x0400,
ERROR_INVERTER_OVER_TEMP = 0x0800,
ERROR_CURRENT_UNSTABLE = 0x1000
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,
ERROR_CURRENT_SENSE_SATURATION = 0x0400,
ERROR_INVERTER_OVER_TEMP = 0x0800,
ERROR_CURRENT_UNSTABLE = 0x1000
};
enum MotorType_t {
@@ -37,41 +37,41 @@ public:
float phC;
};
struct CurrentControl_t{
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
struct CurrentControl_t {
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint; // [A]
float Iq_measured; // [A]
float Id_measured; // [A]
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint; // [A]
float Iq_measured; // [A]
float Id_measured; // [A]
float I_measured_report_filter_k;
float max_allowed_current; // [A]
float overcurrent_trip_level; // [A]
float max_allowed_current; // [A]
float overcurrent_trip_level; // [A]
};
// NOTE: for gimbal motors, all units of A are instead V.
// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
struct Config_t {
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
int32_t pole_pairs = 7;
float calibration_current = 10.0f; // [A]
float resistance_calib_max_voltage = 2.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
int32_t direction = 0; // 1 or -1 (0 = unspecified)
MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
int32_t pole_pairs = 7;
float calibration_current = 10.0f; // [A]
float resistance_calib_max_voltage = 2.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
int32_t direction = 0; // 1 or -1 (0 = unspecified)
MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// float current_lim = 70.0f; //[A]
float current_lim = 10.0f; //[A]
float current_lim = 10.0f; //[A]
float current_lim_tolerance = 1.25f; // multiple of current_lim
// Value used to compute shunt amplifier gains
float requested_current_range = 60.0f; // [A]
float requested_current_range = 60.0f; // [A]
float current_control_bandwidth = 1000.0f; // [rad/s]
float inverter_temp_limit_lower = 100;
float inverter_temp_limit_upper = 120;
@@ -98,8 +98,8 @@ public:
};
Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
Config_t& config);
const GateDriverHardwareConfig_t& gate_driver_config,
Config_t& config);
bool arm();
void disarm();
@@ -130,48 +130,47 @@ public:
const MotorHardwareConfig_t& hw_config_;
const GateDriverHardwareConfig_t gate_driver_config_;
Config_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Axis* axis_ = nullptr; // set by Axis constructor
//private:
//private:
DRV8301_Obj gate_driver_; // initialized in constructor
DRV8301_Obj gate_driver_; // initialized in constructor
uint16_t next_timings_[3] = {
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2
};
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 };
TIM_1_8_PERIOD_CLOCKS / 2};
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = {0};
// variables exposed on protocol
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;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
ArmedState_t armed_state_ = ARMED_STATE_DISARMED;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
CurrentControl_t current_control_ = {
.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
.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,
.Id_measured = 0.0f,
.I_measured_report_filter_k = 1.0f,
.max_allowed_current = 0.0f,
.overcurrent_trip_level = 0.0f,
.Ibus = 0.0f,
.final_v_alpha = 0.0f,
.final_v_beta = 0.0f,
.Iq_setpoint = 0.0f,
.Iq_measured = 0.0f,
.Id_measured = 0.0f,
.I_measured_report_filter_k = 1.0f,
.max_allowed_current = 0.0f,
.overcurrent_trip_level = 0.0f,
};
DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
float thermal_current_lim_ = 10.0f; //[A]
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
float thermal_current_lim_ = 10.0f; //[A]
// Communication protocol definitions
auto make_protocol_definitions() {
@@ -187,59 +186,55 @@ public:
make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_),
make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp),
make_protocol_object("current_control",
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
make_protocol_property("Ibus", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("Id_measured", &current_control_.Id_measured),
make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
),
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
make_protocol_property("Ibus", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("Id_measured", &current_control_.Id_measured),
make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)),
make_protocol_object("gate_driver",
make_protocol_ro_property("drv_fault", &drv_fault_)
// make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
// make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
// make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
// make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value)
),
make_protocol_ro_property("drv_fault", &drv_fault_)
// make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
// make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
// make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
// make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value)
),
make_protocol_object("timing_log",
make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]),
make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]),
make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]),
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])
),
make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]),
make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]),
make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]),
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])),
make_protocol_object("config",
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
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_tolerance", &config_.current_lim_tolerance),
make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower),
make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper),
make_protocol_property("requested_current_range", &config_.requested_current_range),
make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth,
[](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this)
)
);
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
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_tolerance", &config_.current_lim_tolerance),
make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower),
make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper),
make_protocol_property("requested_current_range", &config_.requested_current_range),
make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth,
[](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this)));
}
};
DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t)
#endif // __MOTOR_HPP
#endif // __MOTOR_HPP
Firmware/MotorControl/nvm_config.hpp
+13 -15
View File
@@ -11,9 +11,8 @@
#include <stdlib.h>
#include <stm32f405xx.h>
#include "nvm.h"
#include <fibre/crc.hpp>
#include "nvm.h"
/* Private defines -----------------------------------------------------------*/
#define CONFIG_CRC16_INIT 0xabcd
@@ -33,7 +32,6 @@ static constexpr uint16_t config_version = 0x0001;
/* 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.
@@ -43,10 +41,10 @@ static constexpr uint16_t config_version = 0x0001;
// - 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>
template <typename... Ts>
struct Config;
template<>
template <>
struct Config<> {
static size_t get_size() {
return 0;
@@ -59,7 +57,7 @@ struct Config<> {
}
};
template<typename T, typename ... Ts>
template <typename T, typename... Ts>
struct Config<T, Ts...> {
static size_t get_size() {
return sizeof(T) + Config<Ts...>::get_size();
@@ -71,13 +69,13 @@ struct Config<T, Ts...> {
// 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) {
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))
if (NVM_read(offset, (uint8_t*)val0, size))
return -1;
*crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(previous_crc16, (uint8_t *)val0, size);
*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;
@@ -89,13 +87,13 @@ struct Config<T, Ts...> {
// 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) {
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))
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);
*crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(*crc16, (uint8_t*)val0, size);
if (Config<Ts...>::store_config(offset + size, crc16, vals...))
return -1;
return 0;
@@ -103,7 +101,7 @@ struct Config<T, Ts...> {
// @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) {
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;
@@ -122,7 +120,7 @@ struct Config<T, Ts...> {
// 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) {
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())
@@ -132,7 +130,7 @@ struct Config<T, Ts...> {
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))
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;
Firmware/MotorControl/sensorless_estimator.cpp
+6 -8
View File
@@ -1,9 +1,7 @@
#include "odrive_main.h"
SensorlessEstimator::SensorlessEstimator(Config_t& config) :
config_(config)
{};
SensorlessEstimator::SensorlessEstimator(Config_t& config) : config_(config){};
bool SensorlessEstimator::update() {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
@@ -37,10 +35,10 @@ bool SensorlessEstimator::update() {
}
// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage;
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage;
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
float bandwidth_factor = 1.0f / pm_flux_sqr;
float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
@@ -71,9 +69,9 @@ bool SensorlessEstimator::update() {
// predict PLL phase with velocity
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_);
// update PLL phase with observer permanent magnet phase
phase_ = fast_atan2(eta[1], eta[0]);
phase_ = fast_atan2(eta[1], eta[0]);
float delta_phase = wrap_pm_pi(phase_ - pll_pos_);
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase);
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp * delta_phase);
// update PLL velocity
vel_estimate_ += current_meas_period * pll_ki * delta_phase;
Firmware/MotorControl/sensorless_estimator.hpp
+16 -18
View File
@@ -2,35 +2,35 @@
#define __SENSORLESS_ESTIMATOR_HPP
class SensorlessEstimator {
public:
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
};
struct Config_t {
float observer_gain = 1000.0f; // [rad/s]
float pll_bandwidth = 1000.0f; // [rad/s]
float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
float observer_gain = 1000.0f; // [rad/s]
float pll_bandwidth = 1000.0f; // [rad/s]
float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
};
explicit SensorlessEstimator(Config_t& config);
bool update();
Axis* axis_ = nullptr; // set by Axis constructor
Axis* axis_ = nullptr; // set by Axis constructor
Config_t& config_;
// TODO: expose on protocol
Error_t error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float vel_estimate_ = 0.0f; // [rad/s]
Error_t error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float vel_estimate_ = 0.0f; // [rad/s]
// float pll_kp_ = 0.0f; // [rad/s / rad]
// float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
bool estimator_good_ = false;
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
bool estimator_good_ = false;
// Communication protocol definitions
auto make_protocol_definitions() {
@@ -42,11 +42,9 @@ public:
// make_protocol_property("pll_kp", &pll_kp_),
// make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("observer_gain", &config_.observer_gain),
make_protocol_property("pll_bandwidth", &config_.pll_bandwidth),
make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage)
)
);
make_protocol_property("observer_gain", &config_.observer_gain),
make_protocol_property("pll_bandwidth", &config_.pll_bandwidth),
make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage)));
}
};
Firmware/MotorControl/trapTraj.cpp
+20 -20
View File
@@ -19,13 +19,13 @@ TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config)
bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
float Vmax, float Amax, float Dmax) {
float dX = Xf - Xi; // Distance to travel
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
Ar_ = s * Amax; // Maximum Acceleration (signed)
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
Vr_ = s * Vmax; // Maximum Velocity (signed)
float dX = Xf - Xi; // Distance to travel
float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
Ar_ = s * Amax; // Maximum Acceleration (signed)
Dr_ = -s * Dmax; // Maximum Deceleration (signed)
Vr_ = s * Vmax; // Maximum Velocity (signed)
// If we start with a speed faster than cruising, then we need to decel instead of accel
// aka "double deceleration move" in the paper
@@ -39,12 +39,12 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
// Integral of velocity ramps over the full accel and decel times to get
// minimum displacement required to reach cuising speed
float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
float dXmin = 0.5f * Ta_ * (Vr_ + Vi) + 0.5f * Td_ * Vr_;
// Are we displacing enough to reach cruising speed?
if (s*dX < s*dXmin) {
if (s * dX < s * dXmin) {
// Short move (triangle profile)
Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
Vr_ = s * sqrtf((Dr_ * SQ(Vi) + 2 * Ar_ * Dr_ * dX) / (Dr_ - Ar_));
Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
Td_ = std::max(0.0f, -Vr_ / Dr_);
Tv_ = 0.0f;
@@ -54,11 +54,11 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
}
// Fill in the rest of the values used at evaluation-time
Tf_ = Ta_ + Tv_ + Td_;
Xi_ = Xi;
Xf_ = Xf;
Vi_ = Vi;
yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
Tf_ = Ta_ + Tv_ + Td_;
Xi_ = Xi;
Xf_ = Xf;
Vi_ = Vi;
yAccel_ = Xi + Vi * Ta_ + 0.5f * Ar_ * SQ(Ta_); // pos at end of accel phase
return true;
}
@@ -70,17 +70,17 @@ TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
trajStep.Yd = Vi_;
trajStep.Ydd = 0.0f;
} else if (t < Ta_) { // Accelerating
trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
trajStep.Yd = Vi_ + Ar_*t;
trajStep.Y = Xi_ + Vi_ * t + 0.5f * Ar_ * SQ(t);
trajStep.Yd = Vi_ + Ar_ * t;
trajStep.Ydd = Ar_;
} else if (t < Ta_ + Tv_) { // Coasting
trajStep.Y = yAccel_ + Vr_*(t - Ta_);
trajStep.Y = yAccel_ + Vr_ * (t - Ta_);
trajStep.Yd = Vr_;
trajStep.Ydd = 0.0f;
} else if (t < Tf_) { // Deceleration
float td = t - Tf_;
trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td);
trajStep.Yd = Dr_*td;
trajStep.Y = Xf_ + 0.5f * Dr_ * SQ(td);
trajStep.Yd = Dr_ * td;
trajStep.Ydd = Dr_;
} else if (t >= Tf_) { // Final Condition
trajStep.Y = Xf_;
Firmware/MotorControl/trapTraj.hpp
+8 -10
View File
@@ -2,13 +2,13 @@
#define _TRAP_TRAJ_H
class TrapezoidalTrajectory {
public:
public:
struct Config_t {
float vel_limit = 20000.0f; // [count/s]
float accel_limit = 5000.0f; // [count/s^2]
float decel_limit = 5000.0f; // [count/s^2]
float vel_limit = 20000.0f; // [count/s]
float accel_limit = 5000.0f; // [count/s^2]
float decel_limit = 5000.0f; // [count/s^2]
};
struct Step_t {
float Y;
float Yd;
@@ -23,11 +23,9 @@ public:
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_object("config",
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("accel_limit", &config_.accel_limit),
make_protocol_property("decel_limit", &config_.decel_limit)
)
);
make_protocol_property("vel_limit", &config_.vel_limit),
make_protocol_property("accel_limit", &config_.accel_limit),
make_protocol_property("decel_limit", &config_.decel_limit)));
}
Axis* axis_ = nullptr; // set by Axis constructor
Firmware/MotorControl/utils.c
+19 -23
View File
@@ -1,10 +1,9 @@
#include <utils.h>
#include <math.h>
#include <float.h>
#include <cmsis_os.h>
#include <float.h>
#include <math.h>
#include <stm32f4xx_hal.h>
#include <utils.h>
int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
int Sextant;
@@ -13,30 +12,30 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
if (alpha >= 0.0f) {
//quadrant I
if (one_by_sqrt3 * beta > alpha)
Sextant = 2; //sextant v2-v3
Sextant = 2; //sextant v2-v3
else
Sextant = 1; //sextant v1-v2
Sextant = 1; //sextant v1-v2
} else {
//quadrant II
if (-one_by_sqrt3 * beta > alpha)
Sextant = 3; //sextant v3-v4
Sextant = 3; //sextant v3-v4
else
Sextant = 2; //sextant v2-v3
Sextant = 2; //sextant v2-v3
}
} else {
if (alpha >= 0.0f) {
//quadrant IV
if (-one_by_sqrt3 * beta > alpha)
Sextant = 5; //sextant v5-v6
Sextant = 5; //sextant v5-v6
else
Sextant = 6; //sextant v6-v1
Sextant = 6; //sextant v6-v1
} else {
//quadrant III
if (one_by_sqrt3 * beta > alpha)
Sextant = 4; //sextant v4-v5
Sextant = 4; //sextant v4-v5
else
Sextant = 5; //sextant v5-v6
Sextant = 5; //sextant v5-v6
}
}
@@ -116,9 +115,7 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
// if any of the results becomes NaN, result_valid will evaluate to false
int result_valid =
*tA >= 0.0f && *tA <= 1.0f
&& *tB >= 0.0f && *tB <= 1.0f
&& *tC >= 0.0f && *tC <= 1.0f;
*tA >= 0.0f && *tA <= 1.0f && *tB >= 0.0f && *tB <= 1.0f && *tC >= 0.0f && *tC <= 1.0f;
return result_valid ? 0 : -1;
}
@@ -149,7 +146,7 @@ float fast_atan2(float y, float x) {
// Evaluate polynomials using Fused Multiply Add intrisic instruction.
// coeffs[0] is highest order, as per numpy.polyfit
// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
float horner_fma(float x, const float *coeffs, size_t count) {
float horner_fma(float x, const float* coeffs, size_t count) {
float result = 0.0f;
for (int idx = 0; idx < count; ++idx)
result = fmaf(result, x, coeffs[idx]);
@@ -157,7 +154,7 @@ float horner_fma(float x, const float *coeffs, size_t count) {
}
// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
int mod(int dividend, int divisor){
int mod(int dividend, int divisor) {
int r = dividend % divisor;
return (r < 0) ? (r + divisor) : r;
}
@@ -166,7 +163,7 @@ int mod(int dividend, int divisor){
// If the deadline has already passed, the return value is 0 (except if
// the deadline is very far in the past)
uint32_t deadline_to_timeout(uint32_t deadline_ms) {
uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
uint32_t timeout_ms = deadline_ms - now_ms;
return (timeout_ms & 0x80000000) ? 0 : timeout_ms;
}
@@ -188,18 +185,17 @@ int is_in_the_future(uint32_t time_ms) {
uint32_t micros(void) {
register uint32_t ms, cycle_cnt;
do {
ms = HAL_GetTick();
ms = HAL_GetTick();
cycle_cnt = TIM_TIME_BASE->CNT;
} while (ms != HAL_GetTick());
} while (ms != HAL_GetTick());
return (ms * 1000) + cycle_cnt;
}
// @brief: Busy wait delay for given amount of microseconds (us)
void delay_us(uint32_t us)
{
void delay_us(uint32_t us) {
uint32_t start = micros();
while (micros() - start < (uint32_t) us) {
while (micros() - start < (uint32_t)us) {
__ASM("nop");
}
}
Firmware/MotorControl/utils.h
+3 -3
View File
@@ -6,8 +6,8 @@
extern "C" {
#endif
#include <stdint.h>
#include <math.h>
#include <stdint.h>
/**
* @brief Flash size register address
@@ -67,7 +67,7 @@ extern "C" {
static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
static const float sqrt3_by_2 = 0.86602540378f;
static const float sqrt3_by_2 = 0.86602540378f;
//beware of inserting large values!
static inline float wrap_pm(float x, float pm_range) {
@@ -93,7 +93,7 @@ static inline float fmodf_pos(float x, float y) {
// as per the magnitude invariant clarke transform
// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
// Returns 0 on success, and -1 if the input was out of range
int SVM(float alpha, float beta, float* tA, float* tB, float* tC);
int SVM(float alpha, float beta, float *tA, float *tB, float *tC);
float fast_atan2(float y, float x);
float horner_fma(float x, const float *coeffs, size_t count);