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

remove aligned formatting of assignments

Browse Source
This commit is contained in:
Samuel Sadok
2020-05-20 12:27:28 +02:00
parent 3d710a5385
commit 3d07325022
10 changed files with 60 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+1 -1
View File
@@ -87,7 +87,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, stack_size_ / sizeof(StackType_t));
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
Firmware/MotorControl/axis.hpp
+3 -3
View File
@@ -139,10 +139,10 @@ public:
bool watchdog_check();
void clear_errors() {
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
error_ = Axis::ERROR_NONE;
}
Firmware/MotorControl/controller.cpp
+2 -2
View File
@@ -165,8 +165,8 @@ bool Controller::update(float* current_setpoint_output) {
} break;
case INPUT_MODE_CURRENT_RAMP: {
float max_step_size = std::abs(current_meas_period * config_.current_ramp_rate);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
float full_step = input_current_ - current_setpoint_;
float step = std::clamp(full_step, -max_step_size, max_step_size);
current_setpoint_ += step;
} break;
Firmware/MotorControl/controller.hpp
+22 -22
View File
@@ -44,34 +44,34 @@ public:
bool calib_anticogging = false;
float calib_pos_threshold = 1.0f;
float calib_vel_threshold = 1.0f;
float cogging_ratio = 1.0f;
bool enable = true;
float cogging_ratio = 1.0f;
bool enable = true;
} 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)]
// 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] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
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 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] Infinity to disable.
float vel_limit_tolerance = 1.2f; // ratio to vel_lim. Infinity to disable.
float vel_ramp_rate = 10000.0f; // [(counts/s) / s]
float current_ramp_rate = 1.0f; // A / sec
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;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
float gain_scheduling_width = 10.0f;
bool enable_gain_scheduling = false;
bool enable_vel_limit = true;
bool enable_overspeed_error = true;
bool enable_current_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
};
explicit Controller(Config_t& config);
Firmware/MotorControl/encoder.cpp
+19 -19
View File
@@ -109,8 +109,8 @@ void Encoder::set_linear_count(int32_t count) {
uint32_t prim = cpu_enter_critical();
// Update states
shadow_count_ = count;
pos_estimate_ = static_cast<float>(count);
shadow_count_ = count;
pos_estimate_ = static_cast<float>(count);
tim_cnt_sample_ = count;
//Write hardware last
@@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = static_cast<float>(count_in_cpr_);
pos_cpr_ = static_cast<float>(count_in_cpr_);
cpu_exit_critical(prim);
}
@@ -182,7 +182,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 * static_cast<float>(current_meas_hz));
static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast<float>(current_meas_hz));
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -219,7 +219,7 @@ bool Encoder::run_offset_calibration() {
// scan forward
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) - config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast<float>(i) / static_cast<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);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -248,9 +248,9 @@ 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 / static_cast<float>(config_.cpr));
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast<float>(config_.cpr));
float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
return false;
@@ -259,7 +259,7 @@ bool Encoder::run_offset_calibration() {
// scan backwards
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast<float>(i) / static_cast<float>(num_steps) + config_.calib_scan_distance / 2.0f);
float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast<float>(i) / static_cast<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);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
@@ -273,8 +273,8 @@ bool Encoder::run_offset_calibration() {
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 = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
config_.offset_float = static_cast<float>(residual) / static_cast<float>(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase
is_ready_ = true;
@@ -334,12 +334,12 @@ bool Encoder::abs_spi_init(){
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;
if (mode_ == MODE_SPI_ABS_AEAT) {
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
}
HAL_SPI_DeInit(spi);
HAL_SPI_Init(spi);
@@ -508,9 +508,9 @@ bool Encoder::update() {
pos_estimate_ += current_meas_period * vel_estimate_;
pos_cpr_ += current_meas_period * vel_estimate_;
// discrete phase detector
float delta_pos = static_cast<float>(shadow_count_) - static_cast<int32_t>(std::floor(pos_estimate_));
float delta_pos = static_cast<float>(shadow_count_) - static_cast<int32_t>(std::floor(pos_estimate_));
float delta_pos_cpr = static_cast<float>(count_in_cpr_) - static_cast<int32_t>(std::floor(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast<float>(config_.cpr));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast<float>(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
@@ -518,7 +518,7 @@ bool Encoder::update() {
vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr;
bool snap_to_zero_vel = false;
if (std::abs(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;
}
@@ -544,7 +544,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 / static_cast<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
+4 -4
View File
@@ -8,10 +8,10 @@
class Encoder {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_POLEPAIRS_MISMATCH = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_NONE = 0,
ERROR_UNSTABLE_GAIN = 0x01,
ERROR_CPR_POLEPAIRS_MISMATCH = 0x02,
ERROR_NO_RESPONSE = 0x04,
ERROR_UNSUPPORTED_ENCODER_MODE = 0x08,
ERROR_ILLEGAL_HALL_STATE = 0x10,
ERROR_INDEX_NOT_FOUND_YET = 0x20,
Firmware/MotorControl/endstop.hpp
+2 -2
View File
@@ -42,8 +42,8 @@ class Endstop {
}
private:
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
bool pin_state_ = false;
float pos_when_pressed_ = 0.0f;
Timer<float> debounceTimer_;
};
#endif
Firmware/MotorControl/motor.cpp
+2 -2
View File
@@ -329,7 +329,7 @@ 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 v_beta = c*v_q + s*v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
@@ -400,7 +400,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
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;
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;
Firmware/communication/can_helpers.hpp
+2 -2
View File
@@ -26,7 +26,7 @@ struct can_Signal_t {
template <typename T>
T can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t length, const bool isIntel) {
uint64_t tempVal = 0;
uint64_t mask = (1ULL << length) - 1;
uint64_t mask = (1ULL << length) - 1;
if (isIntel) {
std::memcpy(&tempVal, msg.buf, sizeof(tempVal));
@@ -50,7 +50,7 @@ float can_getSignal(can_Message_t msg, const uint8_t startBit, const uint8_t len
template <typename T>
void can_setSignal(can_Message_t& msg, const T& val, const uint8_t startBit, const uint8_t length, const bool isIntel, const float factor, const float offset) {
T scaledVal = (val - offset) / factor;
T scaledVal = (val - offset) / factor;
uint64_t valAsBits = 0;
std::memcpy(&valAsBits, &scaledVal, sizeof(scaledVal));
Firmware/communication/can_simple.cpp
+3 -3
View File
@@ -279,14 +279,14 @@ void CANSimple::get_encoder_count_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::set_input_pos_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_pos_ = can_getSignal<int32_t>(msg, 0, 32, true);
axis->controller_.input_vel_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.1f, 0);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 48, 16, true, 0.01f, 0);
axis->controller_.input_pos_updated();
}
void CANSimple::set_input_vel_callback(Axis* axis, can_Message_t& msg) {
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_vel_ = can_getSignal<int32_t>(msg, 0, 32, true, 0.01f, 0.0f);
axis->controller_.input_current_ = can_getSignal<int16_t>(msg, 32, 16, true, 0.01f, 0.0f);
}