From a6587f8d8ae1441a924dd3769b823379465f9b37 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 11 Nov 2020 21:02:25 +0100 Subject: [PATCH] fix various issues - overcurrent error during motor calibration (this is caused by overshoot. For now we just ignore the current limit during motor calibration as we did before) - factor 2 error at encoder calibration - set error flag if Motor::arm() is called while the brake resistor is enabled but disarmed. - only arm brake resistor if enabled - auto-arm brake resistor on clear_errors() --- Firmware/MotorControl/encoder.cpp | 8 ++++---- Firmware/MotorControl/low_level.cpp | 7 ++++++- Firmware/MotorControl/main.cpp | 3 +++ Firmware/MotorControl/motor.cpp | 11 ++++++++--- Firmware/MotorControl/open_loop_controller.cpp | 2 +- Firmware/MotorControl/open_loop_controller.hpp | 1 + 6 files changed, 23 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 7c40a185..7132100e 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -228,7 +228,7 @@ bool Encoder::run_offset_calibration() { axis_->open_loop_controller_.target_voltage_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : axis_->motor_.config_.calibration_current; axis_->open_loop_controller_.target_vel_ = 0.0f; axis_->open_loop_controller_.total_distance_ = 0.0f; - axis_->open_loop_controller_.phase_ = wrap_pm_pi(0 - config_.calib_scan_distance / 2.0f); + axis_->open_loop_controller_.phase_ = axis_->open_loop_controller_.initial_phase_ = wrap_pm_pi(0 - config_.calib_scan_distance / 2.0f); axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL); axis_->motor_.current_control_.Idq_setpoint_src_.connect_to(&axis_->open_loop_controller_.Idq_setpoint_); @@ -324,9 +324,9 @@ bool Encoder::run_offset_calibration() { axis_->motor_.disarm(); - config_.phase_offset = encvaluesum / (num_steps * 2); - int32_t residual = encvaluesum - ((int64_t)config_.phase_offset * (int64_t)(num_steps * 2)); - config_.phase_offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase + config_.phase_offset = encvaluesum / num_steps; + int32_t residual = encvaluesum - ((int64_t)config_.phase_offset * (int64_t)num_steps); + config_.phase_offset_float = (float)residual / (float)num_steps + 0.5f; // add 0.5 to center-align state to phase is_ready_ = true; return true; diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 4c9cf23e..f0a2bb64 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -76,6 +76,9 @@ bool brake_resistor_saturated = false; // @brief Arms the brake resistor void safety_critical_arm_brake_resistor() { CRITICAL_SECTION() { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i].motor_.I_bus_ = 0.0f; + } brake_resistor_armed = true; htim2.Instance->CCR3 = 0; htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; @@ -164,7 +167,9 @@ void start_adc_pwm() { HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); - safety_critical_arm_brake_resistor(); + if (odrv.config_.enable_brake_resistor) { + safety_critical_arm_brake_resistor(); + } } // @brief ADC1 measurements are written to this buffer by DMA diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index ae24e3d4..49a91efd 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -161,6 +161,9 @@ void ODrive::clear_errors() { axis.error_ = Axis::ERROR_NONE; } error_ = ERROR_NONE; + if (odrv.config_.enable_brake_resistor) { + safety_critical_arm_brake_resistor(); + } } extern "C" { diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 557cde9c..a81cdc2a 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -198,8 +198,10 @@ bool Motor::arm(PhaseControlLaw<3>* control_law) { control_law_->reset(); } - if (brake_resistor_armed) { + if (!odrv.config_.enable_brake_resistor || brake_resistor_armed) { is_armed_ = true; + } else { + error_ |= Motor::ERROR_BRAKE_RESISTOR_DISARMED; } } @@ -217,7 +219,7 @@ bool Motor::arm(PhaseControlLaw<3>* control_law) { */ void Motor::apply_pwm_timings(uint16_t timings[3], bool tentative) { CRITICAL_SECTION() { - if (!brake_resistor_armed) { + if (odrv.config_.enable_brake_resistor && !brake_resistor_armed) { disarm_with_error(ERROR_BRAKE_RESISTOR_DISARMED); } @@ -609,7 +611,10 @@ void Motor::current_meas_cb(uint32_t timestamp, std::optional current float Inorm_sq = 2.0f / 3.0f * (SQ(current_meas_->phA) + SQ(current_meas_->phB) + SQ(current_meas_->phC)); - if (Inorm_sq > SQ(Itrip)) { + + // Hack: we disable the current check during motor calibration because + // it tends to briefly overshoot when the motor moves to align flux with I_alpha + if (Inorm_sq > SQ(Itrip) && (axis_->current_state_ != Axis::AXIS_STATE_MOTOR_CALIBRATION)) { disarm_with_error(ERROR_CURRENT_LIMIT_VIOLATION); } } else if (is_armed_) { diff --git a/Firmware/MotorControl/open_loop_controller.cpp b/Firmware/MotorControl/open_loop_controller.cpp index 3fe9dc24..16d2d5db 100644 --- a/Firmware/MotorControl/open_loop_controller.cpp +++ b/Firmware/MotorControl/open_loop_controller.cpp @@ -5,7 +5,7 @@ void OpenLoopController::update(uint32_t timestamp) { auto [prev_Id, prev_Iq] = Idq_setpoint_.get_previous().value_or(float2D{0.0f, 0.0f}); auto [prev_Vd, prev_Vq] = Vdq_setpoint_.get_previous().value_or(float2D{0.0f, 0.0f}); - float phase = phase_.get_previous().value_or(0.0f); + float phase = phase_.get_previous().value_or(initial_phase_); float phase_vel = phase_vel_.get_previous().value_or(0.0f); (void)prev_Iq; // unused diff --git a/Firmware/MotorControl/open_loop_controller.hpp b/Firmware/MotorControl/open_loop_controller.hpp index 82356a23..54371bd3 100644 --- a/Firmware/MotorControl/open_loop_controller.hpp +++ b/Firmware/MotorControl/open_loop_controller.hpp @@ -18,6 +18,7 @@ public: float target_vel_ = 0.0f; float target_current_ = 0.0f; float target_voltage_ = 0.0f; + float initial_phase_ = 0.0f; // State/Outputs uint32_t timestamp_ = 0;