From 50ecdef8608bb334ba7f2d2232449e29c7ab2b8e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 5 May 2020 18:16:06 +0200 Subject: [PATCH] move DC overcurrent/undercurrent checks to update_brake_current() The checks now take into account the brake current. Expose ibus on fibre. --- Firmware/MotorControl/axis.cpp | 15 -------- Firmware/MotorControl/axis.hpp | 2 -- Firmware/MotorControl/low_level.cpp | 44 ++++++++++++++++-------- Firmware/MotorControl/low_level.h | 1 + Firmware/MotorControl/motor.hpp | 2 ++ Firmware/MotorControl/odrive_main.h | 9 +++-- Firmware/communication/communication.cpp | 1 + tools/odrive/enums.py | 5 +-- 8 files changed, 43 insertions(+), 36 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 862c027e..b68ef01b 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -166,21 +166,6 @@ bool Axis::do_checks() { if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) error_ |= ERROR_DC_BUS_OVER_VOLTAGE; - // This is the same math that's used in update_brake_current(). Should we calculate IBus globally? - float Ibus_sum = 0.0f; - for (size_t i = 0; i < AXIS_COUNT; ++i) { - if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { - Ibus_sum += axes[i]->motor_.current_control_.Ibus; - } - } - - if (Ibus_sum > board_config.dc_max_positive_current) { - error_ |= ERROR_DC_BUS_OVER_CURRENT; - } - if (Ibus_sum < board_config.dc_max_negative_current) { - error_ |= ERROR_DC_BUS_OVER_REGEN_CURRENT; - } - // Sub-components should use set_error which will propegate to this error_ motor_.do_checks(); // encoder_.do_checks(); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 5ccf0c58..5b489a93 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -24,8 +24,6 @@ public: ERROR_MIN_ENDSTOP_PRESSED = 0x1000, ERROR_MAX_ENDSTOP_PRESSED = 0x2000, ERROR_ESTOP_REQUESTED = 0x4000, - ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000, // too much current pushed into the power supply - ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing }; diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index afd1a5fa..454fa339 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -35,6 +35,7 @@ const float adc_ref_voltage = 3.3f; // 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 ibus_ = 0.0f; // exposed for monitoring only bool brake_resistor_armed = false; bool brake_resistor_saturated = false; /* Private constant data -----------------------------------------------------*/ @@ -600,23 +601,38 @@ void update_brake_current() { brake_duty += std::fmax((vbus_voltage - board_config.dc_bus_overvoltage_ramp_start) / (board_config.dc_bus_overvoltage_ramp_end - board_config.dc_bus_overvoltage_ramp_start), 0.0f); } - if (!std::isnan(brake_duty)) { - if (brake_duty >= 0.95f) { - brake_resistor_saturated = true; - } - - // Duty limit at 95% to allow bootstrap caps to charge - brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); - - // If brake_duty is NaN, this expression will also evaluate to false - int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - safety_critical_apply_brake_resistor_timings(low_off, high_on); - } else { + if (std::isnan(brake_duty)) { // Shuts off all motors AND brake resistor, sets error code on all motors. low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN); + return; } + + if (brake_duty >= 0.95f) { + brake_resistor_saturated = true; + } + + // Duty limit at 95% to allow bootstrap caps to charge + brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); + + // Special handling to avoid the case 0.0/0.0 == NaN. + Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / board_config.brake_resistance) : 0.0f; + + ibus_ = Ibus_sum; + + if (Ibus_sum > board_config.dc_max_positive_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT); + return; + } + if (Ibus_sum < board_config.dc_max_negative_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT); + return; + } + + // If brake_duty is NaN, this expression will also evaluate to false + int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + safety_critical_apply_brake_resistor_timings(low_off, high_on); } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 43a3ac9b..f0b72ecb 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,6 +22,7 @@ extern const float adc_full_scale; extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; +extern float ibus_; extern bool brake_resistor_armed; extern bool brake_resistor_saturated; extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b0f1cef1..aba285cc 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -25,6 +25,8 @@ public: ERROR_INVERTER_OVER_TEMP = 0x0800, ERROR_CURRENT_LIMIT_VIOLATION = 0x1000, ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000, + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000, // too much current pushed into the power supply + ERROR_DC_BUS_OVER_CURRENT = 0x8000, // too much current pulled out of the power supply }; enum MotorType_t { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index f659a431..ade4416c 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -101,11 +101,14 @@ struct BoardConfig_t { * the ODrive will sink more power than usual into the the brake resistor * in an attempt to bring the voltage down again. * - * This setting is active even if all motors are disarmed. - * * The brake duty cycle is increased by the following amount: * vbus_voltage == dc_bus_overvoltage_ramp_start => brake_duty_cycle += 0% * vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100% + * + * Remarks: + * - This setting is active even if all motors are disarmed. + * - brake_resistance must be non-zero, otherwise this will immediately + * result in overcurrent */ bool enable_dc_bus_overvoltage_ramp = false; float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. @@ -116,7 +119,7 @@ struct BoardConfig_t { //!< otherwise the ramp feature is disabled. float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source - float dc_max_negative_current = -INFINITY; // Max current [A] the power supply can sink + float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable. PWMMapping_t pwm_mappings[GPIO_COUNT]; PWMMapping_t analog_mappings[GPIO_COUNT]; }; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 582a4617..fb033bdd 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -116,6 +116,7 @@ public: static inline auto make_obj_tree() { return make_protocol_member_list( make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("ibus", &ibus_), make_protocol_ro_property("serial_number", &serial_number), make_protocol_ro_property("hw_version_major", &hw_version_major), make_protocol_ro_property("hw_version_minor", &hw_version_minor), diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 99b21867..1198272e 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -32,8 +32,6 @@ class errors: ERROR_MIN_ENDSTOP_PRESSED = 0x1000 ERROR_MAX_ENDSTOP_PRESSED = 0x2000 ERROR_ESTOP_REQUESTED = 0x4000 - ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x8000 - ERROR_DC_BUS_OVER_CURRENT = 0x10000 ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000 class motor: @@ -50,6 +48,9 @@ class errors: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 ERROR_CURRENT_SENSE_SATURATION = 0x0400 ERROR_CURRENT_LIMIT_VIOLATION = 0x1000 + ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000 + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000 + ERROR_DC_BUS_OVER_CURRENT = 0x8000 class encoder: ERROR_NONE = 0