From 3d073250222fafb28a7f12a3096846f362b38c03 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 20 May 2020 12:27:28 +0200 Subject: [PATCH] remove aligned formatting of assignments --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/axis.hpp | 6 ++-- Firmware/MotorControl/controller.cpp | 4 +-- Firmware/MotorControl/controller.hpp | 44 +++++++++++++------------- Firmware/MotorControl/encoder.cpp | 38 +++++++++++----------- Firmware/MotorControl/encoder.hpp | 8 ++--- Firmware/MotorControl/endstop.hpp | 4 +-- Firmware/MotorControl/motor.cpp | 4 +-- Firmware/communication/can_helpers.hpp | 4 +-- Firmware/communication/can_simple.cpp | 6 ++-- 10 files changed, 60 insertions(+), 60 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 1d416e68..cb704404 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -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; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 020cd12a..6be3c60c 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -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; } diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index beb93528..ed9163f7 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -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; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 56a60020..4d207a83 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -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)] - 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)] + 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); diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 77b809aa..e0e29a0b 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -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(count); + shadow_count_ = count; + pos_estimate_ = static_cast(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(count_in_cpr_); + pos_cpr_ = static_cast(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(current_meas_hz)); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast(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(i) / static_cast(num_steps) - config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast(i) / static_cast(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(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(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(i) / static_cast(num_steps) + config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast(i) / static_cast(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(residual) / static_cast(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(shadow_count_) - static_cast(std::floor(pos_estimate_)); + float delta_pos = static_cast(shadow_count_) - static_cast(std::floor(pos_estimate_)); float delta_pos_cpr = static_cast(count_in_cpr_) - static_cast(std::floor(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(config_.cpr)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(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(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); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 28830c38..fcf94101 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -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, diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index 6983fa6c..e9cb23ab 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -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 debounceTimer_; }; #endif \ No newline at end of file diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 223aba1c..faafb7f5 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -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; diff --git a/Firmware/communication/can_helpers.hpp b/Firmware/communication/can_helpers.hpp index b5e40385..779b6a7a 100644 --- a/Firmware/communication/can_helpers.hpp +++ b/Firmware/communication/can_helpers.hpp @@ -26,7 +26,7 @@ struct can_Signal_t { template 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 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)); diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 7182d27a..b00b13e0 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -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(msg, 0, 32, true); - axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); + axis->controller_.input_pos_ = can_getSignal(msg, 0, 32, true); + axis->controller_.input_vel_ = can_getSignal(msg, 32, 16, true, 0.1f, 0); axis->controller_.input_current_ = can_getSignal(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(msg, 0, 32, true, 0.01f, 0.0f); + axis->controller_.input_vel_ = can_getSignal(msg, 0, 32, true, 0.01f, 0.0f); axis->controller_.input_current_ = can_getSignal(msg, 32, 16, true, 0.01f, 0.0f); }