From 678c1c9c7e4d2021d0941165d2a16b2b08524901 Mon Sep 17 00:00:00 2001 From: PAJohnson Date: Wed, 12 May 2021 21:39:48 -0400 Subject: [PATCH] Spinout detection fixes - I^2 * R compensation --- Firmware/MotorControl/controller.cpp | 18 +++++++++++++----- Firmware/MotorControl/controller.hpp | 7 ++++--- Firmware/MotorControl/foc.cpp | 2 +- Firmware/odrive-interface.yaml | 22 ++++++++++++++-------- tools/odrive/enums.py | 2 +- 5 files changed, 33 insertions(+), 18 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 7b73e93f..4941cac5 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -364,16 +364,24 @@ bool Controller::update() { } } - mechanical_power_ += config_.mechanical_power_bandwidth * current_meas_period * (torque * *vel_estimate - mechanical_power_); - electrical_power_ += config_.electrical_power_bandwidth * current_meas_period * (axis_->motor_.current_control_.power_ - electrical_power_); + float ideal_electrical_power = 0.0f; + if (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL) { + ideal_electrical_power = axis_->motor_.current_control_.power_ - \ + axis_->motor_.current_control_.Iq_measured_ * axis_->motor_.current_control_.Iq_measured_ * axis_->motor_.config_.phase_resistance - \ + axis_->motor_.current_control_.Id_measured_ * axis_->motor_.current_control_.Id_measured_ * axis_->motor_.config_.phase_resistance; + } + else { + ideal_electrical_power = axis_->motor_.current_control_.power_; + } + mechanical_power_ += config_.mechanical_power_bandwidth * current_meas_period * (torque * *vel_estimate * M_PI * 2.0f - mechanical_power_); + electrical_power_ += config_.electrical_power_bandwidth * current_meas_period * (ideal_electrical_power - electrical_power_); // Spinout check // If mechanical power is negative (braking) and measured power is positive, something is wrong // This indicates that the controller is trying to stop, but torque is being produced. // Usually caused by an incorrect encoder offset - if (mechanical_power_ < 0 && electrical_power_ > config_.spinout_power_margin) { - axis_->encoder_.error_ |= Encoder::ERROR_INCORRECT_OFFSET; - set_error(ERROR_NONE); + if (mechanical_power_ < config_.spinout_mechanical_power_threshold && electrical_power_ > config_.spinout_electrical_power_threshold) { + set_error(ERROR_SPINOUT_DETECTED); return false; } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index f6e589f2..39f4cd0c 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -51,9 +51,10 @@ public: float mirror_ratio = 1.0f; float torque_mirror_ratio = 0.0f; uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration(). Set to -1 to select sensorless estimator. - float mechanical_power_bandwidth = 100.0f; // [rad/s] filter cutoff for mechanical power for spinout detction - float electrical_power_bandwidth = 200.0f; // [rad/s] filter cutoff for electrical power for spinout detection - float spinout_power_margin = 10.0f; // [W] margin for spinout detection + float mechanical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for mechanical power for spinout detction + float electrical_power_bandwidth = 20.0f; // [rad/s] filter cutoff for electrical power for spinout detection + float spinout_electrical_power_threshold = 10.0f; // [W] electrical power threshold for spinout detection + float spinout_mechanical_power_threshold = -10.0f; // [W] mechanical power threshold for spinout detection // custom setters Controller* parent; diff --git a/Firmware/MotorControl/foc.cpp b/Firmware/MotorControl/foc.cpp index efe07996..c02e474d 100644 --- a/Firmware/MotorControl/foc.cpp +++ b/Firmware/MotorControl/foc.cpp @@ -140,7 +140,7 @@ ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output( mod_q = V_to_mod * (Vq + v_current_control_integral_q_ + Ierr_q * p_gain); // calculate power estimate - power_ = Idq->first * v_current_control_integral_d_ + Idq->second * v_current_control_integral_q_; + power_ = Id * (Vd + v_current_control_integral_d_) + Iq * (Vq + v_current_control_integral_q_); // Vector modulation saturation, lock integrator if saturated // TODO make maximum modulation configurable diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 95ead47c..dec1c51c 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -710,6 +710,7 @@ interfaces: Ibeta_measured: {type: readonly float32, c_getter: 'Ialpha_beta_measured_.value_or(float2D{0.0f, 0.0f}).second'} Id_measured: readonly float32 Iq_measured: readonly float32 + power: readonly float32 v_current_control_integral_d: float32 v_current_control_integral_q: float32 final_v_alpha: readonly float32 @@ -830,6 +831,13 @@ interfaces: INVALID_LOAD_ENCODER: INVALID_ESTIMATE: INVALID_CIRCULAR_RANGE: + SPINOUT_DETECTED: + doc: | + The motor mechanical power and electrical power do not agree. This is usually + caused by a slipping encoder or incorrect encoder offset calibration. + Check that your encoder is not slipping on the motor. If using an Index pin, check + that you are not getting false index pulses caused by noise. This can happen if you + are using unshielded cable for the encoder signals. last_error_time: float32 input_pos: type: float32 @@ -924,9 +932,13 @@ interfaces: type: float32 doc: "Bandwidth for electrical power estimate. Used for spinout detection. Dot product of Vdq and Idq" unit: rad/s - spinout_power_margin: + spinout_mechanical_power_threshold: type: float32 - doc: "Margin on electrical power for spinout detection. Should be positive values" + doc: "Mechanical power threshold for spinout detection. This should be a negative value" + unit: Watt + spinout_electrical_power_threshold: + type: float32 + doc: "Electrical power threshold for spinout detection. This should be a positive value" unit: Watt autotuning: c_is_class: False @@ -992,12 +1004,6 @@ interfaces: ABS_SPI_COM_FAIL: ABS_SPI_NOT_READY: HALL_NOT_CALIBRATED_YET: - INCORRECT_OFFSET: - doc: | - Detected by spinout detection. See Controller class config for spinout_power_margin. - Check that your encoder is not slipping on the motor. If using an Index pin, check - that you are not getting false index pulses caused by noise. This can happen if you - are using unshielded cable for the encoder signals. is_ready: readonly bool index_found: readonly bool shadow_count: readonly int32 diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 3506dfbd..292ee3be 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -142,6 +142,7 @@ CONTROLLER_ERROR_INVALID_MIRROR_AXIS = 0x00000008 CONTROLLER_ERROR_INVALID_LOAD_ENCODER = 0x00000010 CONTROLLER_ERROR_INVALID_ESTIMATE = 0x00000020 CONTROLLER_ERROR_INVALID_CIRCULAR_RANGE = 0x00000040 +CONTROLLER_ERROR_SPINOUT_DETECTED = 0x00000080 # ODrive.Encoder.Error ENCODER_ERROR_NONE = 0x00000000 @@ -155,7 +156,6 @@ ENCODER_ERROR_ABS_SPI_TIMEOUT = 0x00000040 ENCODER_ERROR_ABS_SPI_COM_FAIL = 0x00000080 ENCODER_ERROR_ABS_SPI_NOT_READY = 0x00000100 ENCODER_ERROR_HALL_NOT_CALIBRATED_YET = 0x00000200 -ENCODER_ERROR_INCORRECT_OFFSET = 0x00000400 # ODrive.SensorlessEstimator.Error SENSORLESS_ESTIMATOR_ERROR_NONE = 0x00000000