From ae9a208f1fe4a84d5389d075df9b3158bca6ae43 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 5 Apr 2018 22:27:20 -0700 Subject: [PATCH] turn error enums into flags --- Firmware/MotorControl/axis.cpp | 25 +++++++------ Firmware/MotorControl/axis.hpp | 37 ++++++++++--------- Firmware/MotorControl/encoder.cpp | 6 +-- Firmware/MotorControl/encoder.hpp | 10 +++-- Firmware/MotorControl/low_level.cpp | 12 ++++-- Firmware/MotorControl/low_level.h | 2 +- Firmware/MotorControl/motor.cpp | 14 +++---- Firmware/MotorControl/motor.hpp | 19 ++++++---- Firmware/MotorControl/odrive_main.hpp | 11 ++++++ .../MotorControl/sensorless_estimator.cpp | 2 +- .../MotorControl/sensorless_estimator.hpp | 6 ++- 11 files changed, 85 insertions(+), 59 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f9696284..fe3760aa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -101,9 +101,9 @@ bool Axis::check_PSU_brownout() { // Sets error and returns false otherwise. bool Axis::do_checks() { if (!motor_.do_checks()) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, false; + return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; return true; } @@ -115,7 +115,7 @@ bool Axis::run_sensorless_spin_up() { float I_mag = config_.spin_up_current * x; x += current_meas_period / config_.ramp_up_time; if (!motor_.update(I_mag, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; }); if (error_ != ERROR_NO_ERROR) @@ -129,7 +129,7 @@ bool Axis::run_sensorless_spin_up() { phase = wrap_pm_pi(phase + vel * current_meas_period); float I_mag = config_.spin_up_current; if (!motor_.update(I_mag, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); return error_ == ERROR_NO_ERROR; @@ -142,16 +142,16 @@ bool Axis::run_sensorless_control_loop() { float pos_estimate, vel_estimate, phase, current_setpoint; if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) - return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false; + return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // We update the encoder just in case someone needs the output for testing encoder_.update(nullptr, nullptr, nullptr); if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; + return error_ |= ERROR_SENSORLESS_ESTIMATOR_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -166,11 +166,11 @@ bool Axis::run_closed_loop_control_loop() { // We update the sensorless estimator just in case someone needs the output for testing sensorless_estimator_.update(nullptr, nullptr, nullptr); if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_ENCODER_FAILED, false; + return error_ |= ERROR_ENCODER_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -180,6 +180,7 @@ bool Axis::run_closed_loop_control_loop() { bool Axis::run_idle_loop() { // run_control_loop ignores missed modulation timing updates // if and only if we're in AXIS_STATE_IDLE + safety_critical_disarm_motor_pwm(motor_); run_control_loop([this](){ sensorless_estimator_.update(nullptr, nullptr, nullptr); encoder_.update(nullptr, nullptr, nullptr); @@ -276,7 +277,7 @@ void Axis::run_state_machine_loop() { break; default: - error_ = ERROR_INVALID_STATE; + error_ |= ERROR_INVALID_STATE; status = false; // this will set the state to idle break; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c7f909e0..fe5ff15d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -43,17 +43,17 @@ struct AxisConfig_t { class Axis { public: enum Error_t { - ERROR_NO_ERROR = 0, - ERROR_INVALID_STATE = 1, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { - if (motor_.error_ != Motor::ERROR_NO_ERROR) { - error_ = ERROR_MOTOR_FAILED; - break; - } if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { // motor got disarmed in something other than the idle loop - error_ = ERROR_CONTROL_LOOP_TIMEOUT; + error_ |= ERROR_MOTOR_DISARMED; + break; + } + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ |= ERROR_MOTOR_FAILED; break; } @@ -127,7 +127,7 @@ public: // safe and float the phases safety_critical_disarm_motor_pwm(motor_); update_brake_current(); - error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT; + error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } } @@ -190,4 +190,7 @@ public: } }; + +DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t) + #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 8fae0d95..19a39792 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -150,7 +150,7 @@ bool Encoder::run_offset_calibration() { float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error_ = ERROR_CPR_OUT_OF_RANGE; + error_ |= ERROR_CPR_OUT_OF_RANGE; return false; } // check direction @@ -162,7 +162,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.config_.direction = -1; } else { // Encoder response error - error_ = ERROR_RESPONSE; + error_ |= ERROR_RESPONSE; return false; } @@ -192,7 +192,7 @@ bool Encoder::run_offset_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2edf5be0..ae4b3c0e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -22,10 +22,10 @@ struct EncoderConfig_t { class Encoder { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, - ERROR_CPR_OUT_OF_RANGE, - ERROR_RESPONSE, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, + ERROR_CPR_OUT_OF_RANGE = 0x02, + ERROR_RESPONSE = 0x04, }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -83,4 +83,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t) + #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 01f8a392..98ac5620 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -110,11 +110,14 @@ void safety_critical_arm_motor_pwm(Motor& motor) { // After calling this function, it is guaranteed that all three // motor phases are floating and will not be enabled again until // safety_critical_arm_motor_phases is called. -void safety_critical_disarm_motor_pwm(Motor& motor) { +// @returns true if the motor was in a state other than disarmed before +bool safety_critical_disarm_motor_pwm(Motor& motor) { uint8_t sr = cpu_enter_critical(); + bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED; motor.armed_state_ = Motor::ARMED_STATE_DISARMED; __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); cpu_exit_critical(sr); + return was_armed; } // @brief Updates the phase timings unless the motor is disarmed. @@ -303,7 +306,7 @@ void low_level_fault(Motor::Error_t error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { safety_critical_disarm_motor_pwm(axes[i]->motor_); - axes[i]->motor_.error_ = error; + axes[i]->motor_.error_ |= error; } safety_critical_disarm_brake_resistor(); @@ -355,7 +358,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { if (!other_axis.motor_.next_timings_valid_) { // the motor control loop failed to update the timings in time // we must assume that it died and therefore float all phases - safety_critical_disarm_motor_pwm(other_axis.motor_); + bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_); + if (was_armed) { + other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED; + } } else { other_axis.motor_.next_timings_valid_ = false; safety_critical_apply_motor_pwm_timings( diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 5f4a7cde..2bd9fe04 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,7 +22,7 @@ extern "C" { /* Exported functions --------------------------------------------------------*/ void safety_critical_arm_motor_pwm(Motor& motor); -void safety_critical_disarm_motor_pwm(Motor& motor); +bool safety_critical_disarm_motor_pwm(Motor& motor); void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]); void safety_critical_arm_brake_resistor(); void safety_critical_disarm_brake_resistor(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index cf0e80f9..0229e697 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -39,7 +39,7 @@ bool Motor::arm() { // that we have exactly one full interrupt period until the third trigger. This gives // the control loop the correct time quota to set up modulation timings. if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) - return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; + return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; safety_critical_arm_motor_pwm(*this); return true; @@ -119,7 +119,7 @@ bool Motor::check_DRV_fault() { bool Motor::do_checks() { if (!check_DRV_fault()) { - error_ = ERROR_DRV_FAULT; + error_ |= ERROR_DRV_FAULT; return false; } return true; @@ -162,7 +162,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { float Ialpha = -(current_meas_.phB + current_meas_.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage || test_voltage < -max_voltage) - return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A if (!enqueue_voltage_timings(test_voltage, 0.0f)) @@ -216,14 +216,12 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { config_.phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) - return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } bool Motor::run_calibration() { - error_ = ERROR_NO_ERROR; - float R_calib_max_voltage = config_.resistance_calib_max_voltage; if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) @@ -245,7 +243,7 @@ bool Motor::run_calibration() { bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) - return error_ = ERROR_NUMERICAL, false; + return error_ |= ERROR_NUMERICAL, false; next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); @@ -353,7 +351,7 @@ bool Motor::update(float current_setpoint, float phase) { if(!FOC_voltage(0.0f, current_setpoint, phase)) return false; } else { - error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + error_ |= ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 503b8756..b2a79f88 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -54,14 +54,15 @@ typedef struct { class Motor { public: enum Error_t { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ADC_FAILED, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_BRAKE_CURRENT_OUT_OF_RANGE, - ERROR_NUMERICAL + ERROR_NO_ERROR = 0, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02, + ERROR_ADC_FAILED = 0x04, + ERROR_DRV_FAULT = 0x08, + ERROR_CONTROL_DEADLINE_MISSED = 0x10, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x20, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x40, + ERROR_NUMERICAL = 0x80 }; enum TimingLog_t { @@ -209,4 +210,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t) + #endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 50bf69c8..1b4b8850 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -41,6 +41,17 @@ extern BoardConfig_t board_config; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; +// TODO: move +// this is technically not thread-safe but practically it might be +#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \ +inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) | static_cast>(b)); } \ +inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) & static_cast>(b)); } \ +inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) ^ static_cast>(b)); } \ +inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) |= static_cast>(b)); } \ +inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) &= static_cast>(b)); } \ +inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) ^= static_cast>(b)); } \ +inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_cast>(a)); } + // ODrive specific includes #include diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 0fa3c7fe..4ae081f4 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -24,7 +24,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 740ae8c1..910bc05a 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -4,8 +4,8 @@ class SensorlessEstimator { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, }; SensorlessEstimator(); @@ -40,4 +40,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(SensorlessEstimator::Error_t) + #endif /* __SENSORLESS_ESTIMATOR_HPP */